#!/usr/bin/python
# coding:utf-8
# Copyright 2017, JAKA
# All rights reserved.
import sys
import time
from datetime import datetime
import os
import traceback
import socket, redis, netifaces

# file opreate
import shutil, fcntl

# grpc
import grpc
from google.protobuf import json_format

# 并发
from concurrent import futures
import subprocess, threading

# data struct
import configparser
import ctypes
from xml.dom.minidom import parse
import json, struct, signal, hashlib, re
from enum import Enum

# other 3rd modu
import tempfile

# robot core
import jkzuc

# jakamodule
import jaka_pb2, jaka_pb2_grpc
import tool_function
import ftptransfer
import addon_controller_client as addon
import brd

# other module
from functools import wraps
import addon_controller_client as addon
import subprocess
import multiprocessing
import socket
import struct
import tempfile
import codecs
import platform
from collections import OrderedDict
import pylog
import importlib

importlib.reload(sys)
# sys.setdefaultencoding('utf-8')  # 只能在python2上运行

c = jkzuc.command()
s = jkzuc.stat()
e = jkzuc.error()
redis_conn = redis.Redis(host="127.0.0.1", port=6379)
# addon_executor = ExecutorWrapper()
# addon_running_service_num = 0
TASK_MANUAL_MODE = 1
CABV3 = 4

global CONNECT_FLAG
CONNECT_FLAG = False
JOGCON_WARNING = False
HEARTBEAT = False
IDENTIFY_COMPLETE = 1
CURRENT_TIME = 0
SLEEP_INTERVAL = 5
INI_FILE = jkzuc.jkroot_dir + "/configs/JAKA/usersettings.ini"
MASS = 0
CENTROID = (0, 0, 0)
shared_resource_lock = threading.Lock()

### setControllerBoxTime
IS_SETTING_CONTROLLER_BOX_TIME = 0

FUNCDI_NUM = 15
FUNCDO_NUM = 18
MTN_ABORT = False

MAX_EXTIO_CNT = 8
MAX_EXTIO_DI_NUM = 64
MAX_EXTIO_DO_NUM = 64
MAX_EXTIO_AI_NUM = 32
MAX_EXTIO_AO_NUM = 32
JOG_TIME = time.time()
MAX_SERVICE_NUM = 5

fileOpen = 1

SET_NAME_STDIO_DI = {}
SET_NAME_STDIO_DO = {}
SET_NAME_STDIO_AI = {}
SET_NAME_STDIO_AO = {}

SET_NAME_TIO_DI = {}
SET_NAME_TIO_DO = {}
SET_NAME_TIO_AI = {}
SET_NAME_TIO_AO = {}

SET_NAME_EXTIO_DI = {}
SET_NAME_EXTIO_DO = {}
SET_NAME_EXTIO_AI = {}
SET_NAME_EXTIO_AO = {}

SET_NAME_MB_DI = {}
SET_NAME_MB_DO = {}
SET_NAME_MB_AI = {}
SET_NAME_MB_AO = {}

SET_NAME_PFN_DI = {}
SET_NAME_PFN_DO = {}
SET_NAME_PFN_AI = {}
SET_NAME_PFN_AO = {}

SET_NAME_EIP_DI = {}
SET_NAME_EIP_DO = {}
SET_NAME_EIP_AI = {}
SET_NAME_EIP_AO = {}

EIP_SET_IP = 0
GET_EIP_ENABLE = 0
EIP_AI_INT_NUMBER = 0
EIP_AO_INT_NUMBER = 0

cabMaxDIO = 0
modMaxDIO = 0

remoteCopyState = 0

grpc_logger = pylog.grpc_logger


def has_special_chars(text):
    pattern = r'[!@#$%^&*(),.?":{}|<>]'
    match = re.search(pattern, text)
    return match is not None


def check_net(ip_1, netMask_1, ip_2, netMask_2):
    grpc_logger().info("ip_1: %s, netmask_1: %s" % (ip_1, netMask_1))
    grpc_logger().info("ip_2: %s, netmask_2: %s" % (ip_2, netMask_2))

    netMaskInt_1 = struct.unpack("!I", socket.inet_aton(netMask_1))[
        0
    ]  # socket.inet_ntoa(struct.pack('!I', (1 << 32) - (1 << (32-netMaskBits_1))))
    netMaskInt_2 = struct.unpack("!I", socket.inet_aton(netMask_2))[
        0
    ]  # socket.inet_ntoa(struct.pack('!I', (1 << 32) - (1 << (32-netMaskBits_2))))
    ipInt_1 = struct.unpack("!I", socket.inet_aton(ip_1))[0]
    ipInt_2 = struct.unpack("!I", socket.inet_aton(ip_2))[0]

    if (
        (ipInt_1 & netMaskInt_1 == ipInt_2 & netMaskInt_2)
        or (ipInt_1 & netMaskInt_1 == ipInt_2 & netMaskInt_1)
        or (ipInt_1 & netMaskInt_2 == ipInt_2 & netMaskInt_2)
    ):
        ## 同网段或者是子网段
        return -1
    else:
        return 0


def sim_mode_required(expected_value):
    """
    非仿真模式,允许以进一步执行
    Returns:

    """

    def decorator(func):

        @wraps(func)
        def wrapper(*args, **kwargs):
            if s.sim_mode == expected_value:
                result = func(*args, **kwargs)
            else:
                grpc_logger().debug(
                    "[gRPC Decorater] {} reqiured not be satisfied".format(
                        func.__name__
                    )
                )
                result = jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_UNSUPPORTED_COMMAND_IN_SIM,
                    errormsg="can't do that during sim mode",
                )
            return result

        return wrapper

    return decorator


class MiddlewareException(Exception):

    def __init__(self, reason=""):
        super(Exception, self).__init__(reason)
        self.reason = reason


class DeprecatedGrpcException(Exception):

    def __init__(self, desc=""):
        reason = "deprecated {desc}.".format(desc=desc)
        super(Exception, self).__init(reason)


def update_jkzuc_status():
    try:
        s.poll()
    except Exception as err:
        grpc_logger().error("s.poll :" + repr(err))
        reason = "jkzuc.stat poll err,perhaps NML of robot dead"
        raise MiddlewareException(reason)


def get_jkzuc_error():
    try:
        return e.poll()
    except Exception:  # as err:
        # grpc_logger().error("e.poll: "+repr(err))
        reason = "jkzuc.error poll err,perhaps NML of robot dead"
        raise MiddlewareException(reason)


###### FIXME  SE 定制版本， 目前只提供Zu系列机器人仿真，SE版本序列号无法区分Pro、C、Mc

SIMROBOTMODEL_MAP = [
    [3110099, "Zu 3"],
    [5120099, "Zu 5"],
    [7250099, "Zu 7"],
    [7270099, "Zu 7-L1200"],
    [12100099, "Zu 12"],
    [18100099, "Zu 18"],
    [20120199, "JAKA Zu FT"],
    [5110199, "C5"],
    [7301111, "C7"],
    [12120099, "C12"],
    [5150101, "JAKA Pro 5"],
    [7310101, "JAKA Pro 7"],
    [12130199, "JAKA Pro 12"],
    [16100199, "JAKA Pro 16"],
    [18130099, "JAKA Pro 18"],
    [114150001, "MiniCobo"],
    [121200099, "MiniCobo2.0"],
    [20200099, "JAKA Zu 20"],
    [207100099, "JAKA Atom"],
    [203110099, "JAKA DU_Left"],
    [203120099, "JAKA DU_Right"],
    [335100099, "MAX 35"],
]


# this class is defined to keep keys case sensitive
class MyConfigParser(configparser.ConfigParser):

    def __init__(self, defaults=None):
        configparser.ConfigParser.__init__(self, defaults=defaults)

    def optionxform(self, optionstr):
        return optionstr


# this function is defined to
# def async_call(fn):
#     def wrapper(*args, **kwargs):
#         threading.Thread(target=fn, args=args, kwargs=kwargs).start()
#     return wrapper


def restart_systemd_networkd():
    grpc_logger().debug("[gRPC] restart systemd-networkd")
    try:
        tool_function_ctx = tool_function.ModuleCtx(grpc_logger)
        tool_function.runCmdAsRoot(
            "restartService 'systemd-networkd'", tool_function_ctx
        )
    except Exception:
        grpc_logger().debug("[gRPC] error restart systemd-networkd")


def timer_reboot():
    grpc_logger().debug("[gRPC] timer reboot")
    try:
        tool_function_ctx = tool_function.ModuleCtx(grpc_logger)
        tool_function.runCmdAsRoot("reboot", tool_function_ctx)
    except Exception:
        grpc_logger().debug("[gRPC] error reboot")


def firmwareUpgrade(packageName):  # packageName没有后缀
    update_jkzuc_status()
    file = jkzuc.jkroot_dir + "/scripts/servoUpgrade/" + packageName
    #TODO 根据serial num判断伺服包的合法性。但需要上电保证serailnum是正确的.
    if s.emergency_stop:
        grpc_logger().info("[firmwareUpgrade] ESTOP is invalid!!")
        return jaka_pb2.RPC_ERR_EMERGENCY_STOP, "ESTOP is invalid"

    # 解压文件
    file_path = jkzuc.jkroot_dir + "/scripts/servoUpgrade/" + packageName
    os.chdir(jkzuc.jkroot_dir + "/scripts/servoUpgrade")

    if os.path.isfile(file_path + ".tar.gz"):
        grpc_logger().info("servo_upgrade:: file_path = {}.tar.gz".format(file_path))
        zip_file = packageName + ".tar.gz"
        os.system("tar xzvf %s" % zip_file)
    elif os.path.isfile(file_path + ".zip"):
        grpc_logger().info("servo_upgrade:: file_path = {}.zip".format(file_path))
        zip_file = packageName + ".zip"
        os.system("unzip %s" % zip_file)
    if not os.path.exists(file_path):
        grpc_logger().warn(
            "[gRPC] servo upgrade package doesnot exist: {}".format(file_path)
        )
        return jaka_pb2.RPC_FILE_DIR_NOT_FOUND, "Upgrade package is not exist."
    # 获取目录下所有文件列表
    file_list = os.listdir(file_path)
    # 遍历文件列表，检查是否存在以"hex"或“bin”结尾的文件
    hex_files = [
        file for file in file_list if file.endswith(".hex") or file.endswith(".bin")
    ]
    if hex_files:
        for hex_file in hex_files:
            grpc_logger().debug(hex_file)
    else:
        grpc_logger().warn("[firmwareUpgrade] firmware package invalid!!")
        os.system("rm -rf %s*" % file)
        return (
            jaka_pb2.RPC_INVALID_UPGRADE_PKCG,
            "[firmwareUpgrade] firmware package invalid!!",
        )

    c.servo_upgrade()  # 断开jkzuc对于can的所有控制权
    update_jkzuc_status()
    while s.servo_upGrade_percentage_total != 0:  # =0表示已经断开can
        time.sleep(1)
        update_jkzuc_status()
    # 使用伺服工具
    print("GRPC:: servo_upGrade_percentage_total =  ", s.servo_upGrade_percentage_total)
    os.system("halcmd unload IOAIO")
    ret = 0
    appCmd = "./APP/AppRun  " + file_path
    grpc_logger().info("%s" % appCmd)
    ret = os.system("%s > appRun.txt &" % appCmd)
    grpc_logger.info(ret)
    if ret != 0:
        grpc_logger().debug(
            "[gRPC] servo upgrade package is invalid: {}".format(file_path)
        )
        return jaka_pb2.RPC_INVALID_UPGRADE_PKCG, "Upgrade package is invalid"
    grpc_logger().debug("[gRPC] servo upgrade starting")
    return 0, ""


class OnekeyUpgrade:

    class UpgradeState(Enum):
        IDLE = 0
        QUERY_VER = 1
        WAITING_CONFIRM = 2
        SERVO_UPGRADE = 3
        RC_UPGRADE = 4
        SCB_UPGRADE = 5
        DONE = 6
        ERROR = 7

    class UpgradeObject(Enum):
        UPGRADE_SERVO = 1
        UPGRADE_SCB = 2
        UPGRADE_RC = 4

    UPGRADE_TIMEOUT = 0x0F001A
    UPGRADE_ERROR = 0x0F001B

    __upgradeObj = 0
    __packageDirPath = ""  # 一键安装包解压后的绝对路径,如jkroot_dir/scripts/onepack/JKPKG_C1709_SV1311_SF2412
    __packageName = ""  # 一键安装包名称,从grpc接口传入的
    PACKAGE_PATH = jkzuc.jkroot_dir + "/scripts/onepack/"
    RCINSTALL_PATH = jkzuc.jkroot_dir + "/scripts/jkzuc/"
    FIRMWAREINSTALL_PATH = jkzuc.jkroot_dir + "/scripts/servoUpgrade/"
    CANCEL_ID = 253

    def __init__(self):
        self.__state = self.UpgradeState.IDLE
        self.__progress = 0

    def getProgress(self):
        return int(self.__progress)

    def getError(self):
        return int(self.__error)

    def isRunning(self):
        if self.__state == self.UpgradeState.IDLE:
            return False
        return True

    def triggerUpgrade(self, request, context):
        if self.getState() == self.UpgradeState.IDLE:
            ret, errmsg = self.start(request.str, request.intid)
            if ret == 0:
                return jaka_pb2.Response(errcode=0, intid=254)
            else:
                return jaka_pb2.Response(errcode=ret, errormsg=errmsg)
        elif self.getState() == self.UpgradeState.WAITING_CONFIRM:
            ret, errmsg = self.confirmVersion(request.intid)
            if ret == 0:
                return jaka_pb2.Response(errcode=0, intid=254)
            else:
                return jaka_pb2.Response(errcode=ret, errormsg=errmsg)
        else:
            return jaka_pb2.Response(
                errcode=23, errormsg="onekey upgrade state is not promised"
            )

    def checkUpgradeObj(self, objId, type):
        if objId & type.value:
            return True
        return False

    def clearUpgradeObj(self, objId, type):
        objId = objId & (~type.value)

    def start(self, pkgstr, objId):
        if objId == self.CANCEL_ID:
            return 0, ""
        filePath = self.PACKAGE_ROOT_PATH + pkgstr + ".jaka"
        dirPath = self.PACKAGE_PATH + pkgstr
        grpc_logger().debug("onekey_upgrade:: file_path = {}".format(filePath))

        if not os.path.isfile(filePath):
            grpc_logger().debug("[OneUpgrade] package is not file")
            return 21, "file is invalid"
        # check estop signal
        update_jkzuc_status()
        if s.emergency_stop:
            grpc_logger().debug("[OneUpgrade] ESTOP is invalid!!")
            return 21, "ESTOP is invalid state."
        self.__packageName = pkgstr
        self.__packageDirPath = dirPath
        self.__upgradeObj = objId
        self.__progress = 0
        self.setState(self.UpgradeState.QUERY_VER)
        threading.Thread(target=self.updateProgress).start()
        return 0, ""

    def getVersion(self):
        versionInfo = jaka_pb2.RobotUpgMsg()
        versionInfo.servoVer.old_version = self.__versionInfo["servo"]["old"]
        versionInfo.servoVer.new_version = self.__versionInfo["servo"]["new"]
        versionInfo.scbVer.old_version = self.__versionInfo["scb"]["old"]
        versionInfo.scbVer.new_version = self.__versionInfo["scb"]["new"]
        versionInfo.rcVer.old_version = self.__versionInfo["rc"]["old"]
        versionInfo.rcVer.new_version = self.__versionInfo["rc"]["new"]
        versionInfo.canVer.new_version = "null"
        versionInfo.canVer.old_version = "null"
        return 0, "", versionInfo

    def confirmVersion(self, objId):
        grpc_logger().debug("[OneUpgrade] confirmVersion:{}".format(objId))
        if self.__state == self.UpgradeState.WAITING_CONFIRM:
            if objId == self.CANCEL_ID:
                self.setState(self.UpgradeState.DONE)
            else:
                self.setState(self.UpgradeState.SERVO_UPGRADE)
            return 0, ""
        grpc_logger().debug("[OneUpgrade] state is invalid")
        return 23, "upgrade invalid state"

    def setError(self, errcode):
        self.__error = errcode
        self.__state = self.UpgradeState.ERROR

    def setState(self, target):
        if target == self.UpgradeState.IDLE:
            self.__progress = 0
        self.__state = target

    def startScbUpgrade(self):
        grpc_logger().debug(
            "[OneUpgrade] scb packagePath:{} ".format(self.__packagePath["scb"])
        )
        if self.__packagePath["scb"] == "null" or len(self.__packagePath["scb"]) == 0:
            return 1

        sysInst = "cp -r " + self.__packagePath["scb"] + " " + self.FIRMWAREINSTALL_PATH
        os.system(sysInst)

        packageName = re.findall(
            self.__packageDirPath + "/safety/(.+)", self.__packagePath["scb"]
        )
        grpc_logger().debug("[OneUpgrade] scb packageName:{} ".format(packageName))
        if len(packageName) != 1:
            return 2
        ret, errmsg = firmwareUpgrade(packageName[0])
        return 0

    def startRcUpgrade(self):
        grpc_logger().debug(
            "[OneUpgrade] rc packageName:{} ".format(self.__packagePath["rc"])
        )
        if self.__packagePath["rc"] == "null" or len(self.__packagePath["rc"]) == 0:
            return 1

        # 清理rc安装路径
        sysInst = (
            "rm -rf " + self.RCINSTALL_PATH + " && mkdir -p " + self.RCINSTALL_PATH
        )
        os.system(sysInst)

        # 复制rc文件夹
        sysInst = "cp -r " + self.__packagePath["rc"] + " " + self.RCINSTALL_PATH
        os.system(sysInst)
        return 0

    def selectServoPackage(self, curVer):
        if not self.checkUpgradeObj(
            self.__upgradeObj, self.UpgradeObject.UPGRADE_SERVO
        ):
            return "null", "null"
        path = self.__packageDirPath + "/servo"
        packageType = "ZU"
        if curVer / 10000 == 0:  # ZU
            path += "/ZUC"
            packageType = "ZU"
        else:  # minicobot
            path += "/Minicobot"
            packageType = "Minicobot"

        fileList = os.listdir(path)
        if len(fileList) == 0:
            return "null", "null"
        for f in fileList:
            ver = re.findall("SRVA_(.+?)_", f)
            if len(ver) == 1:
                return path, packageType + ver[0]
        return "null", "null"

    def selectScbPackage(self, curVer):
        if not self.checkUpgradeObj(self.__upgradeObj, self.UpgradeObject.UPGRADE_SCB):
            return "null", "null"
        path = self.__packageDirPath + "/safety"
        if curVer == 3:  # minicobot
            path += "/PSCB"
        else:
            path += "/SCB"

        fileList = os.listdir(path)
        if len(fileList) == 0:
            return "null", "null"
        for f in fileList:
            ver = re.findall("(.+)_", f)
            if len(ver) == 1:
                return path, ver[0]
        return "null", "null"

    def selectRcPackage(self):
        if not self.checkUpgradeObj(self.__upgradeObj, self.UpgradeObject.UPGRADE_RC):
            return "null", "null"
        path = self.__packageDirPath + "/ZUC"
        verInfo = re.findall(self.PACKAGE_PATH + "(.+)", self.__packageDirPath)
        if len(verInfo) == 0:
            return "null", "null"
        rcVerStr = re.findall("_C(.+?)_", verInfo[0])  # JKPKG_CXXXX_SVXXXX_SFXXXX.jaka
        if len(rcVerStr) == 0:
            return path, verInfo[0]
        return path, rcVerStr[0]

    def unzipPackage(self, pkgName):
        filePath = self.PACKAGE_ROOT_PATH + pkgName + ".jaka"
        SLEEP_INTERVAL = 60
        try:
            shutil.rmtree(self.PACKAGE_PATH, True)
            os.mkdir(self.PACKAGE_PATH)
            instStr = "tar -zxvf " + filePath + " -C " + self.PACKAGE_PATH
            os.system(instStr)
            dirPath = self.PACKAGE_PATH + pkgName
            os.remove(filePath)

            # 安装包校验
            if (
                (not os.path.isdir(dirPath + "/safety"))
                or (not os.path.isdir(dirPath + "/safety/PSCB"))
                or (not os.path.isdir(dirPath + "/safety/SCB"))
                or (not os.path.isdir(dirPath + "/servo"))
                or (not os.path.isdir(dirPath + "/servo/ZUC"))
                or (not os.path.isdir(dirPath + "/servo/Minicobot"))
                or (not os.path.isdir(dirPath + "/ZUC"))
            ):
                grpc_logger().debug("[OneUpgrade] directory is invaild")
                SLEEP_INTERVAL = 5
                return self.UPGRADE_ERROR

            os.chdir(dirPath)
            md5Result = os.system("md5sum -c check.md5") >> 8  # 获取真正的返回值需要>>8
            if md5Result != 0:
                grpc_logger().debug("[OneUpgrade] md5check failed")
                return self.UPGRADE_ERROR
        except Exception:
            grpc_logger().debug("[OneUpgrade] package can not be unzip")
            SLEEP_INTERVAL = 5
            return self.UPGRADE_ERROR
        return 0

    def updateProgress(self):
        curState = self.UpgradeState.IDLE
        timeout = 5000
        appDisConnectTimer = 30
        newStateFlg = False
        reqReboot = False
        while True:
            time.sleep(1)
            if curState != self.__state:
                grpc_logger().debug(
                    "change from {} to {}".format(curState, self.__state)
                )
                newStateFlg = True
                curState = self.__state
            else:
                newStateFlg = False

            if curState == self.UpgradeState.QUERY_VER:
                self.__progress = 2
                if newStateFlg:
                    ret = self.unzipPackage(self.__packageName)
                    if ret != 0:
                        shutil.rmtree(self.PACKAGE_PATH, True)
                        self.setError(ret)
                        continue

                    timeout = 30
                    # 设置维护模式，保证在一键升级时候关闭上电自动使能，使能自动执行程序的配置时不会导致checksum发生变换
                    c.set_maintenance_mode(1)
                    # 先缓存上电使能运行程序配置，避免在升级过程中问题
                    update_jkzuc_status()
                    oldAutoEnable = s.auto_status[0]
                    oldAutoProgram = s.auto_status[1]
                    c.set_auto_op(0, 0)
                    c.state(2)
                update_jkzuc_status()
                if s.powered_on and timeout <= 25:
                    # set servo upgrade info
                    servoVerNum = 0
                    self.__versionInfo["servo"]["old"], servoVerNum = (
                        tool_function.getServoVerStr(s.servo_version)
                    )
                    robSerial = s.robot_model_data["model"]
                    self.__packagePath["servo"], self.__versionInfo["servo"]["new"] = (
                        self.selectServoPackage(robSerial)
                    )
                    if robSerial == 710:
                        self.__versionInfo["servo"]["new"] += "(Unsupport 710!)"
                        self.__packagePath["servo"] = "null"
                    elif servoVerNum < 0x2181 and (robSerial / 10000 == 0):  # zu
                        self.__versionInfo["servo"]["old"] += "(Too old)"
                        self.__packagePath["servo"] = "null"
                    elif servoVerNum < 0x2178 and (robSerial / 10000 == 1):  # minicobo
                        self.__versionInfo["servo"]["old"] += "(Too old)"
                        self.__packagePath["servo"] = "null"
                    grpc_logger().debug(
                        "cur servo {} new {} pkg {}".format(
                            self.__versionInfo["servo"]["old"],
                            self.__versionInfo["servo"]["new"],
                            self.__packagePath["servo"],
                        )
                    )

                    # set scb upgrade info
                    self.__versionInfo["scb"]["old"], scbVerNum = (
                        tool_function.getScbVerStr(s)
                    )
                    self.__packagePath["scb"], self.__versionInfo["scb"]["new"] = (
                        self.selectScbPackage(s.cab_type)
                    )
                    if s.cab_type == 3 and scbVerNum < 206:
                        self.__packagePath["scb"] = "null"
                        self.__versionInfo["scb"][
                            "old"
                        ] += "(Too old.At least PSCB_02_06)"
                    elif s.cab_type == 2 and scbVerNum < 255:
                        self.__packagePath["scb"] = "null"
                        self.__versionInfo["scb"][
                            "old"
                        ] += "(Too old.At least SCB_02_55)"
                    elif s.cab_type == 1:
                        self.__packagePath["scb"] = "null"
                        self.__versionInfo["scb"]["old"] = "Unsupport CABV1"
                    grpc_logger().debug(
                        "cur scb {} new {} pkg {}".format(
                            self.__versionInfo["scb"]["old"],
                            self.__versionInfo["scb"]["new"],
                            self.__packagePath["scb"],
                        )
                    )

                    ## set rc upgrade info
                    self.__versionInfo["rc"]["old"] = tool_function.getRcVerStr()
                    self.__packagePath["rc"], self.__versionInfo["rc"]["new"] = (
                        self.selectRcPackage()
                    )
                    grpc_logger().debug(
                        "cur rc {} new {} pkg {}".format(
                            self.__versionInfo["rc"]["old"],
                            self.__versionInfo["rc"]["new"],
                            self.__packagePath["rc"],
                        )
                    )

                    self.__progress = 5
                    self.setState(self.UpgradeState.WAITING_CONFIRM)
                    # 恢复原上电使能运行程序配置
                    c.set_auto_op(oldAutoEnable, oldAutoProgram)
                    c.state(1)
                elif timeout < 0:
                    grpc_logger().debug("onekey upgrade QUERY_VER timeout!!")
                    self.setError(self.UPGRADE_TIMEOUT)
                    # 恢复原上电使能运行程序配置
                    c.set_auto_op(oldAutoEnable, oldAutoProgram)
                    c.state(1)
                    c.set_maintenance_mode(0)
                    continue
                timeout -= 1
            elif curState == self.UpgradeState.WAITING_CONFIRM:
                if newStateFlg:
                    timeout = 180
                    appDisConnectTimer = 30

                if self.isAppDisconnect():
                    appDisConnectTimer -= 1
                else:
                    appDisConnectTimer = 30

                if appDisConnectTimer <= 0:
                    grpc_logger().debug(
                        "app connect timeout when onekey upgrade WAITING_CONFIRM!!"
                    )
                    self.setError(self.UPGRADE_TIMEOUT)
                    self.__progress = 0

                if timeout <= 0:
                    grpc_logger().debug("onekey upgrade WAITING_CONFIRM timeout!!")
                    self.setError(self.UPGRADE_TIMEOUT)
                    self.__progress = 0
                timeout -= 1
            elif curState == self.UpgradeState.SERVO_UPGRADE:
                if newStateFlg:
                    timeout = 180
                    if not self.checkUpgradeObj(
                        self.__upgradeObj, self.UpgradeObject.UPGRADE_SERVO
                    ):
                        grpc_logger().debug("no servo upgrade")
                        self.setState(self.UpgradeState.RC_UPGRADE)
                        continue
                    ret = self.startServoUpgrade()
                    if ret != 0:
                        self.setError(self.UPGRADE_ERROR)
                        continue
                    update_jkzuc_status()
                else:
                    update_jkzuc_status()
                    curProgress = int(
                        5 + float(s.servo_upGrade_percentage_total) / 100.0 * 40.0
                    )
                    if curProgress == 45:
                        self.setState(self.UpgradeState.RC_UPGRADE)
                        reqReboot = True

                    # 若长时间进度未更新则认为异常
                    if self.__progress != curProgress:
                        timeout = 180
                    self.__progress = curProgress
                    if timeout <= 0:
                        self.setError(self.UPGRADE_TIMEOUT)
                        continue
                timeout -= 1
            elif curState == self.UpgradeState.RC_UPGRADE:
                if not self.checkUpgradeObj(
                    self.__upgradeObj, self.UpgradeObject.UPGRADE_RC
                ):
                    grpc_logger().debug("no rc upgrade")
                else:
                    ret = self.startRcUpgrade()
                    if ret != 0:
                        self.setError(self.UPGRADE_ERROR)
                        continue
                self.__progress = 50
                reqReboot = True
                self.setState(self.UpgradeState.SCB_UPGRADE)
            elif curState == self.UpgradeState.SCB_UPGRADE:
                if newStateFlg:
                    if not self.checkUpgradeObj(
                        self.__upgradeObj, self.UpgradeObject.UPGRADE_SCB
                    ):
                        grpc_logger().debug("no scb upgrade")
                        self.__progress = 95
                        self.setState(self.UpgradeState.DONE)
                        continue
                    ret = self.startScbUpgrade()
                    if ret != 0:
                        self.setError(self.UPGRADE_ERROR)
                        continue
                    update_jkzuc_status()
                else:
                    update_jkzuc_status()
                    self.__progress = int(
                        50 + float(s.servo_upGrade_percentage_total) / 100.0 * 45.0
                    )
                    if self.__progress == 95:
                        reqReboot = True
                        time.sleep(1.0)
                        self.setState(self.UpgradeState.DONE)
            elif curState == self.UpgradeState.ERROR:
                time.sleep(2.0)
                self.setState(self.UpgradeState.IDLE)
                if reqReboot == True:
                    threading.Timer(1, timer_reboot).start()
                return
            elif curState == self.UpgradeState.DONE:
                self.__progress = 0
                SLEEP_INTERVAL = 5  # 恢复原来设置
                shutil.rmtree(self.PACKAGE_PATH)
                self.setState(self.UpgradeState.IDLE)
                if reqReboot == True:
                    threading.Timer(1, timer_reboot).start()
                return


class Robot(jaka_pb2_grpc.RobotServicer):
    errorprompt = ("", "登录失败", "Plese Login first" "")
    oneUpgrade = OnekeyUpgrade()
    servoDebugMode = 0
    hbRes = (
        jaka_pb2.Response()
    )  # response for heartbeat, to avoid frequently create/destroy
    client_uq_IPA = ""
    VISITOR_REQ_LIST = [
        jaka_pb2.MSGID_LOGIN,
        jaka_pb2.MSGID_LOGOUT,
        jaka_pb2.MSGID_STATECHANGE,
        jaka_pb2.MSGID_JOG,
        jaka_pb2.MSGID_MOVJ,
        jaka_pb2.MSGID_JOG_MOVL,
        jaka_pb2.MSGID_MTNABORT,
        jaka_pb2.MSGID_HOME,
        jaka_pb2.MSGID_JOGSTOP,
        jaka_pb2.MSGID_HEARTBEAT,
        jaka_pb2.MSGID_GETTCP,
        jaka_pb2.MSGID_GETJOINTLMT,
        jaka_pb2.MSGID_SETVELRATE,
        jaka_pb2.MSGID_EXITPROTECT,
        jaka_pb2.MSGID_GETROBOTPOSE,
        jaka_pb2.MSGID_GETPAYLOAD,
        jaka_pb2.MSGID_SETAUTOWORK,
        jaka_pb2.MSGID_GETZUCVERSION,
        jaka_pb2.MSGID_GETIONAME,
        jaka_pb2.MSGID_GETAUTOWORK,
        jaka_pb2.MSGID_GETCAMLIST,
        jaka_pb2.MSGID_GETDIFUNC,
        jaka_pb2.MSGID_GETDOFUNC,
        jaka_pb2.MSGID_GETTOOLOFFSET,
        jaka_pb2.MSGID_TELEOPENABLE,
        jaka_pb2.MSGID_GETUSERLIST,
        jaka_pb2.MSGID_GETCLSNSENSITIVITY,
        jaka_pb2.MSGID_GETINSTALLATIONANGLE,
        jaka_pb2.MSGID_GETMUTITOOLOFFSET,
        jaka_pb2.MSGID_GETVELRATE,
        jaka_pb2.MSGID_SETCURRENTTOOLID,
        jaka_pb2.MSGID_GETLOCALDIR,
        jaka_pb2.MSGID_GETEXTIOBRIEFINFO,
        jaka_pb2.MSGID_GETEXTIOMODCONFIG,
        jaka_pb2.MSGID_SETMUTIUSEROFFSET,
        jaka_pb2.MSGID_GETMUTIUSEROFFSET,
        jaka_pb2.MSGID_GETROBOTMODELNUM,
        jaka_pb2.MSGID_GETROBOTMODELDATA,
        jaka_pb2.MSGID_GETTORQUESENSORCOMM,
        jaka_pb2.MSGID_GETTORQUESENSORPAYLOAD,
        jaka_pb2.MSGID_SETTORQUESENSORMODE,
        jaka_pb2.MSGID_SETSENSORBRAND,
        jaka_pb2.MSGID_SETADMITCTRLOPTION,
        jaka_pb2.MSGID_GETADMITCTRLOPTION,
        jaka_pb2.MSGID_ENABLEADMITTANCECTRL,
        jaka_pb2.MSGID_GETMODBUSSLAVEID,
        jaka_pb2.MSGID_SETCURRENTUSERID,
        jaka_pb2.MSGID_CALCUSER,
        jaka_pb2.MSGID_GETMODBUSSLAVEPARAM,
        jaka_pb2.MSGID_GETCUSTOMPOSE,
        jaka_pb2.MSGID_GETSYSTEMMONITORDATA,
        jaka_pb2.MSGID_SETEXTIOMODE,
        jaka_pb2.MSGID_PROGRUN,
        jaka_pb2.MSGID_PROGPAUSE,
        jaka_pb2.MSGID_PROGRESUME,
        jaka_pb2.MSGID_GETPAYLOADIDENTIRYSTATUS,
        jaka_pb2.MSGID_GETPAYLOADIDENTIRYTRAJ,
        jaka_pb2.MSGID_QUERYUDV,
        jaka_pb2.MSGID_KINEFORWARD,
        jaka_pb2.MSGID_KINEINVERSE,
        jaka_pb2.MSGID_GETCURRENTFILE,
        jaka_pb2.MSGID_GETCURRENTLINE,
        jaka_pb2.MSGID_SETDOUT,
        jaka_pb2.MSGID_SETAOUT,
        jaka_pb2.MSGID_SETTCP,
        jaka_pb2.MSGID_CHECKFILEMD5,
        jaka_pb2.MSGID_LOADPROGRAM,
        jaka_pb2.MSGID_GETADDONLIST,
        jaka_pb2.MSGID_JOG_MOVL,
        jaka_pb2.MSGID_GETDHPARAMS,
        jaka_pb2.MSGID_GETCONTROLLERBOXTIME,
        jaka_pb2.MSGID_GETANALOGCHANNELSETTING,
        jaka_pb2.MSGID_GETDIGITALCHANNELSETTING,
        jaka_pb2.MSGID_GETBRSTARTVOLTAGE,
        jaka_pb2.MSGID_GETCABVERSION,
        jaka_pb2.MSGID_GETDHCPORSTATICIP,
        jaka_pb2.MSGID_SETTRAJTRACKCONFIG,
        jaka_pb2.MSGID_GETTRAJTRACKCONFIG,
        jaka_pb2.MSGID_GENERATENGCSCRIPT,
        jaka_pb2.MSGID_TRAJSAMPLE,
        jaka_pb2.MSGID_ISINTRAJSAMPLING,
        jaka_pb2.MSGID_RENAMEDIRANDFILE,
        jaka_pb2.MSGID_GETSUBTRAJTRACKCONFIG,
        jaka_pb2.MSGID_GETSUBTRAJTRACKCONFIG,
        jaka_pb2.MSGID_GETAUTOOPTION,
        jaka_pb2.MSGID_GETTIOPINMODE,
        jaka_pb2.MSGID_GETTIOVOUTPARAM,
        jaka_pb2.MSGID_GETTIORS485CHANNELMODE,
        jaka_pb2.MSGID_GETTIORS485CHANNELCOMM,
        jaka_pb2.MSGID_TIORS485UPDATESIGNAL,
        jaka_pb2.MSGID_TIORS485GETSIGNALLIST,
        jaka_pb2.MSGID_GETSAFEZONE,
        jaka_pb2.MSGID_GETEXECUTINGSCTIPTSTATE,
        jaka_pb2.MSGID_SETROBOTSAFETYSETTINGS,
        jaka_pb2.MSGID_GETROBOTSAFETYSETTINGS,
        jaka_pb2.MSGID_GETDEFAULTROBOTSAFETYSETTINGS,
        jaka_pb2.MSGID_SETJOINTSAFETYSETTINGS,
        jaka_pb2.MSGID_GETJOINTSAFETYSETTINGS,
        jaka_pb2.MSGID_GETDEFAULTJOINTSAFETYSETTINGS,
        jaka_pb2.MSGID_GETDISAFETYFUNC,
        jaka_pb2.MSGID_GETDOSAFETYFUNC,
        jaka_pb2.MSGID_GETSCRIPTTHREADSTATE,
        jaka_pb2.MSGID_SETPROFINET,
        jaka_pb2.MSGID_GETPROFINET,
        jaka_pb2.MSGID_GETIOCONFIGINFO,
        jaka_pb2.MSGID_GETIOSTATE,
        jaka_pb2.MSGID_SETEIP,
        jaka_pb2.MSGID_GETEIP,
        jaka_pb2.MSGID_GETCOLLISIONOPTION,
        jaka_pb2.MSGID_GETTIOFREEORPROGBUTTONOPT,
        jaka_pb2.MSGID_GETTIOFREEBUTTONOPT,
        jaka_pb2.MSGID_GETTIOPOINTBUTTONOPT,
        jaka_pb2.MSGID_GETCABVOLTAGE,
        jaka_pb2.MSGID_SETHOMEVELOCITY,
        jaka_pb2.MSGID_GETHOMEVELOCITY,
        jaka_pb2.MSGID_SHUTDOWN,
        jaka_pb2.MSGID_GETCABINETID,
        jaka_pb2.MSGID_GETADDONSERVICESTATUS,
        jaka_pb2.MSGID_GETADMITCTRLCONFIG,
        jaka_pb2.MSGID_GETADMITTANCECTRLFRAME,
        jaka_pb2.MSGID_GETADMITTANCEDRAGDEADZONE,
        jaka_pb2.MSGID_GETCOMPLIANTTYPEENABLE,
        jaka_pb2.MSGID_GETPAYLOADIDENTIRYRESULT,
        jaka_pb2.MSGID_GETREDUCEDMODELEVELVALUE,
        jaka_pb2.MSGID_GETSCRIPTAUTOBACKUP,
        jaka_pb2.MSGID_GETSCRIPTVARIABILE,
        jaka_pb2.MSGID_GETSENSORBRAND,
        jaka_pb2.MSGID_GETTOOLPAYLOAD,
        jaka_pb2.MSGID_GETTORQSENSORPAYLOADIDENTIFYRESULT,
        jaka_pb2.MSGID_GETTORQSENSORPAYLOADIDENTIFYSTATUS,
        jaka_pb2.MSGID_GETTORQSENSORPAYLOADIDENTIFYSTATUS,
        jaka_pb2.MSGID_GETTORQUESENSORSOFTLIMIT,
        jaka_pb2.MSGID_GETTORQUESENSORVALUE,
        jaka_pb2.MSGID_GETUPGRADEINFO,
        jaka_pb2.MSGID_ISROBOTINLIMITEDSTATUS,
        jaka_pb2.MSGID_GETCAMLIST,
        jaka_pb2.MSGID_GETTORQUESENSORFILTER,
        jaka_pb2.MSGID_CALCULATEUSERPOINTTOWORLD,
        jaka_pb2.MSGID_CALCULATEWORLDPOINTTOUSER,
        jaka_pb2.MSGID_SETOPERATIONMODE,
        jaka_pb2.MSGID_GETOPERATIONMODE,
        jaka_pb2.MSGID_GETOPERATIONMODEOPTION,
    ]

    def __init__(self):
        self.clientIP = ""
        self.clientPort = ""
        self.LOGSTATUS = False
        self.username = ""
        self.password = ""
        self.userlevel = ""
        self.use_cmd_torq = ""
        tool_function_ctx = tool_function.ModuleCtx(grpc_logger)
        tempval = tool_function.read_ini_config(
            INI_FILE,
            "ZUC",
            "RPC_USE_CMD_TORQ",
            fallback="",
            module_ctx=tool_function_ctx,
        )
        if tempval == "":
            self.use_cmd_torq = 0
        else:
            self.use_cmd_torq = int(tempval)
        grpc_logger().info("use cmd torq: {}".format(self.use_cmd_torq))
        self.func_name_dict = {
            jaka_pb2.MSGID_LOGIN: self.LogIn,
            jaka_pb2.MSGID_LOGOUT: self.LogOut,
            jaka_pb2.MSGID_STATECHANGE: self.RobotStateChange,
            jaka_pb2.MSGID_MODECHANGE: self.ModeChange,
            jaka_pb2.MSGID_JOG: self.Jog,
            jaka_pb2.MSGID_MOVJ: self.JogMovJ,
            jaka_pb2.MSGID_JOG_MOVL: self.JogMovL,
            jaka_pb2.MSGID_JOGSTOP: self.JogStop,
            jaka_pb2.MSGID_SETDOUT: self.SetDout,
            jaka_pb2.MSGID_SETAOUT: self.SetAout,
            jaka_pb2.MSGID_MTNABORT: self.MtnAbort,
            jaka_pb2.MSGID_SERVOHOME: self.ServoHome,
            jaka_pb2.MSGID_HOME: self.Home,
            jaka_pb2.MSGID_HEARTBEAT: self.HeartBeat,
            jaka_pb2.MSGID_SETTCP: self.SetTcp,
            jaka_pb2.MSGID_GETTCP: self.GetTcp,
            jaka_pb2.MSGID_PROGRUN: self.ProgRun,
            jaka_pb2.MSGID_PROGPAUSE: self.ProgPause,
            jaka_pb2.MSGID_PROGRESUME: self.ProgResume,
            jaka_pb2.MSGID_SETJOINTLMT: self.SetJointLimit,
            jaka_pb2.MSGID_GETJOINTLMT: self.GetJointLimit,
            jaka_pb2.MSGID_SETTRAJMAXVEL: self.SetTrajMaxVel,
            jaka_pb2.MSGID_SETVELRATE: self.SetVelRate,
            jaka_pb2.MSGID_EXITPROTECT: self.ExitProtectStat,
            jaka_pb2.MSGID_GETCURRENTFILE: self.GetCurrentFile,
            jaka_pb2.MSGID_GETCURRENTLINE: self.GetCurrentLine,
            jaka_pb2.MSGID_KINEINVERSE: self.KineInverse,
            jaka_pb2.MSGID_KINEFORWARD: self.KineForward,
            jaka_pb2.MSGID_CALCULATEUSERPOINTTOWORLD: self.CalculateUserPointToWorld,
            jaka_pb2.MSGID_CALCULATEWORLDPOINTTOUSER: self.CalculateWorldPointToUser,
            jaka_pb2.MSGID_CHECKFILEMD5: self.CheckFileMd5,
            jaka_pb2.MSGID_GETROBOTPOSE: self.GetRobotPose,
            jaka_pb2.MSGID_SETPAYLOAD: self.SetPayload,
            jaka_pb2.MSGID_GETPAYLOAD: self.GetPayload,
            jaka_pb2.MSGID_GETZUCVERSION: self.GetZucVersion,
            jaka_pb2.MSGID_UPGRADESYSTEM: self.UpgradeSystem,
            jaka_pb2.MSGID_SETIONAME: self.SetIOName,
            jaka_pb2.MSGID_GETIONAME: self.GetIOName,
            jaka_pb2.MSGID_SETAUTOWORK: self.SetAutoWork,
            jaka_pb2.MSGID_GETAUTOWORK: self.GetAutoWork,
            jaka_pb2.MSGID_SETAUTOOPTION: self.SetAutoOp,
            jaka_pb2.MSGID_GETAUTOOPTION: self.GetAutoOp,
            jaka_pb2.MSGID_CALCTCP: self.CalculateTCP,
            jaka_pb2.MSGID_DELETEFILE: self.DeleteFile,
            jaka_pb2.MSGID_REMOTECOPY: self.RemoteCopy,
            jaka_pb2.MSGID_LOCALCOPY: self.CopyFile,
            jaka_pb2.MSGID_GETCAMLIST: self.GetCameraList,
            jaka_pb2.MSGID_SETDIFUNC: self.SetDIFunc,
            jaka_pb2.MSGID_GETDIFUNC: self.GetDIFunc,
            jaka_pb2.MSGID_SETDOFUNC: self.SetDOFunc,
            jaka_pb2.MSGID_GETDOFUNC: self.GetDOFunc,
            jaka_pb2.MSGID_ADDUDV: self.AddUserDefinedVariable,
            jaka_pb2.MSGID_DELETEUDV: self.DeleteUserDefinedVariable,
            jaka_pb2.MSGID_MODIFYUDV: self.ModifyUserDefinedVariable,
            jaka_pb2.MSGID_QUERYUDV: self.QueryUserDefinedVariable,
            jaka_pb2.MSGID_SHUTDOWN: self.Shutdown,
            jaka_pb2.MSGID_SETTOOLOFFSET: self.SetToolOffset,
            jaka_pb2.MSGID_GETTOOLOFFSET: self.GetToolOffset,
            jaka_pb2.MSGID_TELEOPENABLE: self.TeleopEnable,
            jaka_pb2.MSGID_SETROBOTID: self.SetRobotID,
            jaka_pb2.MSGID_SETROBOTNAME: self.SetRobotName,
            jaka_pb2.MSGID_GETUSERLIST: self.GetUserList,
            jaka_pb2.MSGID_CHANGEUSERINFO: self.ChangeUserInfo,
            jaka_pb2.MSGID_SETCLSNSENSITIVITY: self.SetClsnSensitivity,
            jaka_pb2.MSGID_GETCLSNSENSITIVITY: self.GetClsnSensitivity,
            jaka_pb2.MSGID_SETINSTALLATIONANGLE: self.SetInstallationAngle,
            jaka_pb2.MSGID_GETINSTALLATIONANGLE: self.GetInstallationAngle,
            jaka_pb2.MSGID_SETMUTITOOLOFFSET: self.SetMutiToolOffset,
            jaka_pb2.MSGID_GETMUTITOOLOFFSET: self.GetMutiToolOffset,
            jaka_pb2.MSGID_GETVELRATE: self.GetVelRate,
            jaka_pb2.MSGID_SETCURRENTTOOLID: self.SetCurrentToolId,
            jaka_pb2.MSGID_GETLOCALDIR: self.GetLocalDir,
            jaka_pb2.MSGID_GETEXTIOBRIEFINFO: self.GetExtIOBriefInfo,
            jaka_pb2.MSGID_GETEXTIOMODCONFIG: self.GetExtIOModConfig,
            jaka_pb2.MSGID_SETEXTIOMODE: self.SetExtIOMode,
            jaka_pb2.MSGID_ADDEXTIOMOD: self.AddExtIOMod,
            jaka_pb2.MSGID_MODIFYEXTIOMOD: self.ModifyExtIOMod,
            jaka_pb2.MSGID_DELETEEXTIOMOD: self.DeleteExtIOMod,
            jaka_pb2.MSGID_MOVEEXTIOMOD: self.MoveExtIOMod,
            jaka_pb2.MSGID_SETJOINTBRAKERELEASE: self.SetJointBrakeRelease,
            jaka_pb2.MSGID_SETJOINTMULTITURNSTEP: self.SetJointMultiTurnStep,
            jaka_pb2.MSGID_SERVOUPGRADE: self.ServoUpgrade,
            jaka_pb2.MSGID_GETROBOTMODELNUM: self.GetRobotModelVer,
            jaka_pb2.MSGID_GETSAFEATTITUDE: self.GetSafeAttitude,
            jaka_pb2.MSGID_GETPLANNERJERK: self.GetPlannerJerk,
            jaka_pb2.MSGID_SETMUTIUSEROFFSET: self.SetMutiUserOffset,
            jaka_pb2.MSGID_GETMUTIUSEROFFSET: self.GetMutiUserOffset,
            jaka_pb2.MSGID_SETCURRENTUSERID: self.SetUserFrameId,
            jaka_pb2.MSGID_CALCUSER: self.CalculateUser,
            jaka_pb2.MSGID_GETROBOTMODELDATA: self.GetRobotModelData,
            jaka_pb2.MSGID_SETMODBUSSLAVEID: self.SetModbusSlaveId,
            jaka_pb2.MSGID_GETMODBUSSLAVEID: self.GetModbusSlaveId,
            jaka_pb2.MSGID_SETTORQUESENSORCOMM: self.SetTorqueSensorComm,
            jaka_pb2.MSGID_GETTORQUESENSORCOMM: self.GetTorqueSensorComm,
            jaka_pb2.MSGID_SETTORQUESENSORPAYLOAD: self.SetTorqueSensorPayload,
            jaka_pb2.MSGID_GETTORQUESENSORPAYLOAD: self.GetTorqueSensorPayload,
            jaka_pb2.MSGID_SETTORQUESENSORMODE: self.SetTorqueSensorMode,
            jaka_pb2.MSGID_SETADMITCTRLOPTION: self.SetAdmitControlOption,
            jaka_pb2.MSGID_GETADMITCTRLOPTION: self.GetAdmitControlOption,
            jaka_pb2.MSGID_ENABLEADMITTANCECTRL: self.EnableAdmittanceCtrl,
            jaka_pb2.MSGID_LOADPROGRAM: self.LoadProgram,
            jaka_pb2.MSGID_SETMODBUSSLAVEPARAM: self.SetModbusSlaveParam,
            jaka_pb2.MSGID_GETMODBUSSLAVEPARAM: self.GetModbusSlaveParam,
            jaka_pb2.MSGID_SETCUSTOMPOSE: self.SetCustomPose,
            jaka_pb2.MSGID_GETCUSTOMPOSE: self.GetCustomPose,
            jaka_pb2.MSGID_GETSYSTEMMONITORDATA: self.GetSystemMonitorData,
            jaka_pb2.MSGID_EXPORTZUCSETTINGS: self.ExportZucSettings,
            jaka_pb2.MSGID_IMPORTZUCSETTINGS: self.ImportZucSettings,
            jaka_pb2.MSGID_SETPAYLOADIDENTIRYTRAJSTARTPOS: self.SetPayloadIdentifyTrajStartPos,
            jaka_pb2.MSGID_SETPAYLOADIDENTIRYTRAJENDPOS: self.SetPayloadIdentifyTrajEndPos,
            jaka_pb2.MSGID_GETPAYLOADIDENTIRYTRAJ: self.GetPayloadIdentifyTraj,
            jaka_pb2.MSGID_STARTPAYLOADIDENTIRYTRAJ: self.StartPayloadIdentifyTraj,
            jaka_pb2.MSGID_GETPAYLOADIDENTIRYRESULT: self.GetPayloadIdentifyResult,
            jaka_pb2.MSGID_RESETPAYLOADIDENTIRYSTATUS: self.ResetPayloadIdentifyStatus,
            jaka_pb2.MSGID_GETPAYLOADIDENTIRYSTATUS: self.GetPayloadIdentifyStatus,
            jaka_pb2.MSGID_STARTTORQSENSORPAYLOADIDENTIFY: self.StartTorqSensorPayloadIdentify,
            jaka_pb2.MSGID_GETTORQSENSORPAYLOADIDENTIFYRESULT: self.GetTorqSensorPayloadIdentifyResult,
            jaka_pb2.MSGID_GETTORQSENSORPAYLOADIDENTIFYSTATUS: self.GetTorqSensorIdentifyStaus,
            jaka_pb2.MSGID_GETREDUCEDMODELEVELVALUE: self.GetReducedModeLevelValue,
            jaka_pb2.MSGID_SETREDUCEDMODELEVELVALUE: self.SetReducedModeLevelValue,
            jaka_pb2.MSGID_GETADDONLIST: self.GetAddOnList,
            jaka_pb2.MSGID_DELETEADDON: self.DeleteAddOn,
            jaka_pb2.MSGID_SETADDONSTAUS: self.SetAddOnStaus,
            jaka_pb2.MSGID_INSTALLADDON: self.InstallAddon,
            jaka_pb2.MSGID_SETUPANALOGCHANNEL: self.SetupAnalogChannel,
            jaka_pb2.MSGID_GETANALOGCHANNELSETTING: self.GetAnalogChannelSetting,
            jaka_pb2.MSGID_SETDHPARAMS: self.SetDhParams,
            jaka_pb2.MSGID_SETCONTROLLERBOXTIME: self.SetControllerBoxTime,
            jaka_pb2.MSGID_GETDHPARAMS: self.GetDhParams,
            jaka_pb2.MSGID_GETCONTROLLERBOXTIME: self.GetControllerBoxTime,
            jaka_pb2.MSGID_SETDHCPORSTATICIP: self.SetDhcpOrStaticIP,
            jaka_pb2.MSGID_PROGRAMSTEP: self.ProgramStep,
            jaka_pb2.MSGID_SETSCRIPTAUTOBACKUP: self.SetScriptAutoBackup,
            jaka_pb2.MSGID_GETSCRIPTAUTOBACKUP: self.GetScriptAutoBackup,
            jaka_pb2.MSGID_BACKUPSCRIPT: self.BackupScript,
            jaka_pb2.MSGID_SETUPDIGITALCHANNEL: self.SetupDigitalChannel,
            jaka_pb2.MSGID_GETDIGITALCHANNELSETTING: self.GetDigitalChannelSetting,
            jaka_pb2.MSGID_SETBRSTARTVOLTAGE: self.SetBRStartVoltage,
            jaka_pb2.MSGID_GETBRSTARTVOLTAGE: self.GetBRStartVoltage,
            jaka_pb2.MSGID_GETCABVERSION: self.GetCABVersion,
            jaka_pb2.MSGID_SETTOOLPAYLOAD: self.SetToolPayload,
            jaka_pb2.MSGID_GETTOOLPAYLOAD: self.GetToolPayload,
            jaka_pb2.MSGID_SETTORQUESENSORSOFTLIMIT: self.SetTorqueSensorSoftLimit,
            jaka_pb2.MSGID_GETTORQUESENSORSOFTLIMIT: self.GetTorqueSensorSoftLimit,
            jaka_pb2.MSGID_SETADMITCTRLCONFIG: self.SetAdmitCtrlConfig,
            jaka_pb2.MSGID_GETADMITCTRLCONFIG: self.GetAdmitCtrlConfig,
            jaka_pb2.MSGID_SETTORQUESENSORFILTER: self.SetTorqueSensorFilter,
            jaka_pb2.MSGID_GETTORQUESENSORFILTER: self.GetTorqueSensorFilter,
            jaka_pb2.MSGID_GETTORQUESENSORVALUE: self.GetTorqueSensorValue,
            jaka_pb2.MSGID_AUTOIDENTIFYINITPOINT: self.AutoIdentifyInitPoint,
            jaka_pb2.MSGID_AUTOIDENTIFYTOOlPAYLOAD: self.AutoIdentifyToolPayload,
            jaka_pb2.MSGID_SETSENSORBRAND: self.SetTorsensorBrand,
            jaka_pb2.MSGID_GETSENSORBRAND: self.GetTorsensorBrand,
            jaka_pb2.MSGID_SETCOMPLIANTTYPEENABLE: self.SetCompliantType,
            jaka_pb2.MSGID_GETCOMPLIANTTYPEENABLE: self.GetCompliantType,
            jaka_pb2.MSGID_SETADMITTANCECTRLFRAME: self.SetAdmittanceCtrlFrame,
            jaka_pb2.MSGID_GETADMITTANCECTRLFRAME: self.GetAdmittanceCtrlFrame,
            jaka_pb2.MSGID_DISABLEFORCECONTROL: self.DisableForceControl,
            jaka_pb2.MSGID_SETDHCPORSTATICIP: self.SetDhcpOrStaticIP,
            jaka_pb2.MSGID_GETDHCPORSTATICIP: self.GetDhcpOrStaticIP,
            jaka_pb2.MSGID_SETTRAJTRACKCONFIG: self.SetTrajTrackConfig,
            jaka_pb2.MSGID_GETTRAJTRACKCONFIG: self.GetTrajTrackConfig,
            jaka_pb2.MSGID_GENERATENGCSCRIPT: self.GenarateNgcScript,
            jaka_pb2.MSGID_TRAJSAMPLE: self.TrajSample,
            jaka_pb2.MSGID_ISINTRAJSAMPLING: self.IsInSamplingStatus,
            jaka_pb2.MSGID_RENAMEDIRANDFILE: self.RenameDirAndFile,
            jaka_pb2.MSGID_SETDISAFETYFUNC: self.SetDISafetyFunc,
            jaka_pb2.MSGID_GETDISAFETYFUNC: self.GetDISafetyFunc,
            jaka_pb2.MSGID_SETDOSAFETYFUNC: self.SetDOSafetyFunc,
            jaka_pb2.MSGID_GETDOSAFETYFUNC: self.GetDOSafetyFunc,
            jaka_pb2.MSGID_SETSUBTRAJTRACKCONFIG: self.SetSubTrajTrackConfig,
            jaka_pb2.MSGID_GETSUBTRAJTRACKCONFIG: self.GetSubTrajTrackConfig,
            jaka_pb2.MSGID_SETTIOPINMODE: self.SetTIOPinMode,
            jaka_pb2.MSGID_GETTIOPINMODE: self.GetTIOPinMode,
            jaka_pb2.MSGID_SETTIOVOUTPARAM: self.SetTIOVoutParam,
            jaka_pb2.MSGID_GETTIOVOUTPARAM: self.GetTIOVoutParam,
            jaka_pb2.MSGID_SETTIORS485CHANNELMODE: self.SetTIORS485ChannelMode,
            jaka_pb2.MSGID_GETTIORS485CHANNELMODE: self.GetTIORS485ChannelMode,
            jaka_pb2.MSGID_SETTIORS485CHANNELCOMM: self.SetTIORS485ChannelComm,
            jaka_pb2.MSGID_GETTIORS485CHANNELCOMM: self.GetTIORS485ChannelComm,
            jaka_pb2.MSGID_TIORS485ADDSIGNAL: self.TIORS485AddSignal,
            jaka_pb2.MSGID_TIORS485DELETESIGNAL: self.TIORS485DeleteSignal,
            jaka_pb2.MSGID_TIORS485UPDATESIGNAL: self.TIORS485UpdateSignal,
            jaka_pb2.MSGID_TIORS485GETSIGNALLIST: self.TIORS485GetSignalList,
            jaka_pb2.MSGID_TIORS485SENDCOMMAND: self.TIORS485SendCommand,
            jaka_pb2.MSGID_SETSAFEZONEPARAM: self.SetSafeZone,
            jaka_pb2.MSGID_SETSAFEATTITUDEPARAM: self.SetSafeAttitude,
            jaka_pb2.MSGID_SETPLANNERJERK: self.SetPlannerJerk,
            jaka_pb2.MSGID_GETPLANNERJERK: self.GetPlannerJerk,
            jaka_pb2.MSGID_SETSAFEZONEENABLE: self.SetSafeZoneEnable,
            jaka_pb2.MSGID_GETSAFEZONE: self.GetSafeZone,
            jaka_pb2.MSGID_GETEXECUTINGSCTIPTSTATE: self.GetExecutingScriptState,
            jaka_pb2.MSGID_DIAGNOSISCONTROL: self.DiagnosisControl,
            jaka_pb2.MSGID_SERVODEBUGENABLE: self.ServoDebugEnable,
            jaka_pb2.MSGID_SETROBOTSAFETYSETTINGS: self.SetRobotSafetySettings,
            jaka_pb2.MSGID_GETROBOTSAFETYSETTINGS: self.GetRobotSafetySettings,
            jaka_pb2.MSGID_GETDEFAULTROBOTSAFETYSETTINGS: self.GetDefaultRobotSafetySettings,
            jaka_pb2.MSGID_SETJOINTSAFETYSETTINGS: self.SetJointSafetySettings,
            jaka_pb2.MSGID_GETJOINTSAFETYSETTINGS: self.GetJointSafetySettings,
            jaka_pb2.MSGID_GETDEFAULTJOINTSAFETYSETTINGS: self.GetDefaultJointSafetySettings,
            jaka_pb2.MSGID_SETCOLLISIONOPTION: self.SetCollisionOption,
            jaka_pb2.MSGID_GETCOLLISIONOPTION: self.GetCollisionOption,
            jaka_pb2.MSGID_GETSCRIPTVARIABILE: self.GetScriptVariables,
            jaka_pb2.MSGID_GETSCRIPTTHREADSTATE: self.GetScriptThreadState,
            jaka_pb2.MSGID_GETCABINETID: self.GetCabinetID,
            jaka_pb2.MSGID_ISROBOTINLIMITEDSTATUS: self.IsRobotInLimitedStatus,
            jaka_pb2.MSGID_SETPROFINET: self.SetProfinetConfig,
            jaka_pb2.MSGID_GETPROFINET: self.GetProfinetConfig,
            jaka_pb2.MSGID_SETTIOFREEBUTTONOPT: self.SetFreeButtonOpt,
            jaka_pb2.MSGID_GETTIOFREEBUTTONOPT: self.GetFreeButtonOpt,
            jaka_pb2.MSGID_SETTIOFREEORPROGBUTTONOPT: self.SetFreeProgButtonOpt,
            jaka_pb2.MSGID_GETTIOFREEORPROGBUTTONOPT: self.GetFreeProgButtonOpt,
            jaka_pb2.MSGID_SETTIOPOINTBUTTONOPT: self.SetPointButtonOpt,
            jaka_pb2.MSGID_GETTIOPOINTBUTTONOPT: self.GetPointButtonOpt,
            jaka_pb2.MSGID_SETHOMEVELOCITY: self.SetHomeVelocity,
            jaka_pb2.MSGID_GETIOCONFIGINFO: self.GetIOConfigInfo,
            jaka_pb2.MSGID_GETIOSTATE: self.GetIOState,
            jaka_pb2.MSGID_SETEIP: self.SetEIP,
            jaka_pb2.MSGID_GETEIP: self.GetEIP,
            jaka_pb2.MSGID_STARTMACHINEAUTORUN: self.StartMachineAutoRunAddonService,
            jaka_pb2.MSGID_GETADDONSERVICESTATUS: self.GetAddOnServiceStatus,
            jaka_pb2.MSGID_GETUPGRADEINFO: self.GetUpgradeInfo,
            jaka_pb2.MSGID_GETCABVOLTAGE: self.GetCabVoltage,
            jaka_pb2.MSGID_SETCABVOLTAGE: self.SetCabVoltage,
            jaka_pb2.MSGID_CALUSAFEATTITUDEPARAM: self.CalculateSafeAtt,
            jaka_pb2.MSGID_INSTALLADDONASYNC: self.InstallAddOnAsync,
            jaka_pb2.MSGID_EXPORTADDONFILESASYNC: self.ExportAddOnFilesAsync,
            jaka_pb2.MSGID_SETADMITTANCEDRAGDEADZONE: self.SetAdmittanceDragDeadZone,
            jaka_pb2.MSGID_GETADMITTANCEDRAGDEADZONE: self.GetAdmittanceDragDeadZone,
            jaka_pb2.MSGID_EXPORTADDONFILES: self.ExportAddOnFiles,
            jaka_pb2.MSGID_SETHOMEVELOCITY: self.SetHomeVelocity,
            jaka_pb2.MSGID_GETHOMEVELOCITY: self.GetHomeVelocity,
            jaka_pb2.MSGID_STARTIDENTIFYDYNAMICS: self.StartIdentifyDynamics,
            jaka_pb2.MSGID_STOPIDENTIFYDYNAMICS: self.StopIdentifyDynamics,
            jaka_pb2.MSGID_GETDYNAMICSIDENTIFYSTATUS: self.GetDynamicsIdentifyStatus,
            jaka_pb2.MSGID_GETDYNAMICSIDENTIFYSTARTPOS: self.GetDynamicsIdentifyStartPos,
            jaka_pb2.MSGID_GETDYNAMICSIDENTIFYRESULT: self.GetDynamicsIdentifyResult,
            jaka_pb2.MSGID_GETFRICTIONIDENTIFYRESULT: self.GetFrictionIdentifyResult,
            jaka_pb2.MSGID_STARTIDENTIFYFRICTION: self.StartIdentifyFriction,
            jaka_pb2.MSGID_STOPIDENTIFYFRICTION: self.StopIdentifyFriction,
            jaka_pb2.MSGID_SERVOIDENTIFYPARA: self.SetServoIdentifyPara,
            jaka_pb2.MSGID_SERVO_FRICTION: self.SetServoFrictionPara,
            jaka_pb2.MSGID_SETIDENTIFYFRICTIONSTARTPOS: self.SetIdentifyFrictionStartPos,
            jaka_pb2.MSGID_SETIDENTIFYFRICTIONENDPOS: self.SetIdentifyFrictionEndPos,
            jaka_pb2.MSGID_GETFRICTIONIDENTIFYSTATUS: self.GetFrictionIdentifyStatus,
            jaka_pb2.MSGID_SETROBOTSIMENABLE: self.SetRobotSimEnable,
            jaka_pb2.MSGID_GETROBOTSIMENABLE: self.GetRobotSimEnable,
            jaka_pb2.MSGID_SETSIMROBOT: self.SetSimRobot,
            jaka_pb2.MSGID_GETSIMROBOT: self.GetSimRobot,
            jaka_pb2.MSGID_GETROBOTMODELLIST: self.GetRobotModelList,
            jaka_pb2.MSGID_SETOPERATIONMODE: self.SetOperationMode,
            jaka_pb2.MSGID_GETOPERATIONMODE: self.GetOperationMode,
            jaka_pb2.MSGID_GETOPERATIONMODEOPTION: self.GetOperationModeOption,
        }

    # 自定义装饰器，检查登录状态
    def login_required(func):

        def wrapper(self, request, context):
            # 在这里检查标志位或执行权限验证逻辑
            client_ip = context.peer().split(":")[1]
            if "127.0.0.1" != client_ip:  # if not loop client(addon)
                if not self.LOGSTATUS:
                    context.set_code(grpc.StatusCode.PERMISSION_DENIED)
                    context.set_details("please login first")
                    return ""
            # 如果标志位检查通过，执行原始方法
            return func(self, request, context)

        return wrapper

    def login_admin_required(func):

        def wrapper(self, request, context):
            if self.userlevel != "administrator":
                context.set_code(grpc.StatusCode.PERMISSION_DENIED)
                context.set_details("please login as administrator")
                return ""
            # 如果标志位检查通过，执行原始方法
            return func(self, request, context)

        return wrapper

    def get_folder_size(self, folder_path="."):
        total_size = 0
        for dirpath, dirnames, filenames in os.walk(folder_path):
            for f in filenames:
                fp = os.path.join(dirpath, f)
                # skip if it is symbolic link
                if not os.path.islink(fp):
                    total_size += os.path.getsize(fp)
        return total_size

    def extIOUnifiedIndex2ModIndex(self, typeid, unifiedIdx, extIOConfigs):
        # typeid: 'di': 0, 'do': 1, 'ai': 2, 'ao': 3
        # unifiedIdx: the index unified for all extended IO of the same typeid
        # extIOConfigs: the extIOConfigs for extended IO module

        assert unifiedIdx >= 0
        if typeid == 0 or typeid == 1:  # DI: [0, 63], DO: [0, 63]
            assert unifiedIdx < 64
        elif typeid == 2 or typeid == 3:  # AI: [0, 31], AO: [0, 31]
            assert unifiedIdx < 32

        lastModIOPinMap = extIOConfigs[0][len(extIOConfigs[0]) - 1]
        lastModIOPinSetup = extIOConfigs[1][len(extIOConfigs[1]) - 1]
        totalPinNumOfType = lastModIOPinMap[typeid] + lastModIOPinSetup[3][typeid][1]
        if unifiedIdx < 0 or unifiedIdx >= totalPinNumOfType:
            return (-1, -1)

        i = len(extIOConfigs[0])
        modPinIdx = ()
        for pyIOPinMap in reversed(extIOConfigs[0]):
            i = i - 1
            if unifiedIdx >= pyIOPinMap[typeid]:
                modPinIdx = (i, unifiedIdx - pyIOPinMap[typeid])
                break

        return modPinIdx

    def extIOModIndex2UnifiedIndex(self, typeid, modPinIdx, extIOConfigs):
        # typeid: 'di': 0, 'do': 1, 'ai': 2, 'ao': 3
        # modPinIdx: module index and the pin index on the module, in format of (modIdx, pinIdxInMod)
        # extIOConfigs: the extIOConfigs for extended IO module
        unifiedIdx = -1
        modIdx, pinIdxInMod = modPinIdx
        if modIdx >= 0 and modIdx < len(extIOConfigs[1]):
            pyExtIOSetup = extIOConfigs[1][modIdx]
            pinNumOfTypeInMod = pyExtIOSetup[3][typeid][1]
            if pinIdxInMod >= pinNumOfTypeInMod or pinIdxInMod < 0:
                unifiedIdx = -1
            else:
                unifiedIdx = extIOConfigs[0][modIdx][typeid] + pinIdxInMod
        return unifiedIdx

    def updateExtIOName(self, oldExtIOConfigs, newExtIOConfigs):
        # update the name of extending IO pins as per the new configs
        # newExtIOSetups: list of extIO setups
        try:
            # grpc_logger().info('updateExtIOName')
            # grpc_logger().info('oldExtIOConfigs {}'.format(oldExtIOConfigs))
            # grpc_logger().info('newExtIOConfigs {}'.format(newExtIOConfigs))
            filename = jkzuc.jkroot_dir + "/configs/JAKA/aliasname.ini"
            config = configparser.ConfigParser()
            config.read(filename)
            # find an remove the obsolete pins
            iters = [("EXT_DI", 0), ("EXT_DO", 1), ("EXT_AI", 2), ("EXT_AO", 3)]
            for item in iters:
                sectName, pinType = item
                # obsoletePins = []
                if config.has_section(sectName):
                    # modified_item = {}
                    # grpc_logger().info('13.setion Name: {}'.format(sectName))
                    for key, value in config.items(sectName):
                        modPinIdx = self.extIOUnifiedIndex2ModIndex(
                            pinType, int(key), oldExtIOConfigs
                        )
                        # grpc_logger().info('old: {} {}'.format(int(key), modPinIdx))
                        newUnifiedIdx = self.extIOModIndex2UnifiedIndex(
                            pinType, modPinIdx, newExtIOConfigs
                        )
                        # grpc_logger().info('new: {}'.format(newUnifiedIdx))
                        if newUnifiedIdx >= 0:
                            if newUnifiedIdx != int(key):
                                config.remove_option(sectName, str(key))
                                config.set(sectName, str(newUnifiedIdx), value)
                        else:
                            config.remove_option(sectName, str(key))

            # rewrite the data back to the file
            with open(filename, "w") as fd:
                fcntl.flock(fd, fcntl.LOCK_EX)
                config.write(fd)
                os.fsync(fd)
                fcntl.flock(fd, fcntl.LOCK_UN)
        except:
            grpc_logger().error(traceback.format_exc())
            return

    def updateExtIOName_move(
        self, oldExtIOConfigs, newExtIOConfigs, currIndex, tarIndex
    ):
        # update the name of extending IO pins as per the new configs
        # newExtIOSetups: list of extIO setups
        filename = jkzuc.jkroot_dir + "/configs/JAKA/aliasname.ini"
        try:
            config = configparser.ConfigParser()
            config_new = configparser.ConfigParser()
            config.read(filename)
            config_new.read(filename)
            # find an remove the obsolete pins
            iters = [("EXT_DI", 0), ("EXT_DO", 1), ("EXTAI", 2), ("EXT_AO", 3)]
            for item in iters:
                # sectName ext_di
                sectName, pinType = item
                # obsoletePins = []
                if config_new.has_section(sectName):
                    for key, value in config_new.items(sectName):
                        config_new.remove_option(sectName, str(key))
            for item in iters:
                # sectName ext_di
                sectName, pinType = item
                # obsoletePins = []
                if config.has_section(sectName):
                    for key, value in config.items(sectName):
                        modPinIdx = self.extIOUnifiedIndex2ModIndex(
                            pinType, int(key), oldExtIOConfigs
                        )
                        mod, pinIdx = modPinIdx
                        newMod = -1
                        if currIndex > tarIndex:
                            if mod == currIndex:
                                newMod = mod - (currIndex - tarIndex)
                                ioNum = 0
                                for modNum in range(newMod + 1, currIndex + 1):
                                    ioNum = (
                                        ioNum
                                        + newExtIOConfigs[1][modNum][3][pinType][1]
                                    )
                                newUnifiedIdx = int(key) - ioNum
                            elif mod > currIndex:
                                newMod = mod
                                newUnifiedIdx = int(key)
                            else:
                                if mod < tarIndex:
                                    newMod = mod
                                    newUnifiedIdx = int(key)
                                else:
                                    newMod = mod + 1
                                    newUnifiedIdx = (
                                        int(key)
                                        + oldExtIOConfigs[1][currIndex][3][pinType][1]
                                    )
                        else:
                            # currIndex < tarIndex
                            if mod == currIndex:
                                newMod = mod + (tarIndex - currIndex)
                                ioNum = 0
                                for modNum in range(currIndex, newMod):
                                    ioNum = (
                                        ioNum
                                        + newExtIOConfigs[1][modNum][3][pinType][1]
                                    )
                                newUnifiedIdx = int(key) + ioNum
                            elif mod < currIndex:
                                newMod = mod
                                newUnifiedIdx = int(key)
                            else:
                                if mod <= tarIndex:
                                    newMod = mod - 1
                                    newUnifiedIdx = (
                                        int(key)
                                        - oldExtIOConfigs[1][currIndex][3][pinType][1]
                                    )
                                else:
                                    newMod = mod
                                    newUnifiedIdx = int(key)

                        if newUnifiedIdx >= 0:
                            config_new.set(sectName, str(newUnifiedIdx), value)
                        else:
                            config_new.remove_option(sectName, str(key))
            # rewrite the data back to the file.
            with open(filename, "w") as fd:
                fcntl.flock(fd, fcntl.LOCK_EX)
                config_new.write(fd)
                os.fsync(fd)
                fcntl.flock(fd, fcntl.LOCK_UN)
        except:
            grpc_logger().error(traceback.format_exc())
            return

    def updateExtIOName1(self, oldExtIOConfigs, newExtIOConfigs, modID):
        # update the name of extending IO pins as per the new configs
        # newExtIOSetups: list of extIO setups
        # grpc_logger().info("modID: {}".format(modID))
        # grpc_logger().info("modIDtype: {}".format(type(modID)))

        filename = jkzuc.jkroot_dir + "/configs/JAKA/aliasname.ini"
        try:
            config = configparser.ConfigParser()
            config.read(filename)
            # find an remove the obsolete pins
            iters = [("EXT_DI", 0), ("EXT_DO", 1), ("EXT_AI", 2), ("EXT_AO", 3)]
            for item in iters:
                # sectName ext_di
                sectName, pinType = item
                # obsoletePins = []
                if config.has_section(sectName):
                    for key, value in config.items(sectName):
                        modPinIdx = self.extIOUnifiedIndex2ModIndex(
                            pinType, int(key), oldExtIOConfigs
                        )
                        mod, pinIdx = modPinIdx
                        if mod == modID:
                            newUnifiedIdx = -1
                        elif mod > modID:
                            newKey = int(key) - int(
                                oldExtIOConfigs[1][modID][3][pinType][1]
                            )
                            newUnifiedIdx = newKey
                        else:
                            newUnifiedIdx = int(key)
                        if newUnifiedIdx >= 0:
                            if newUnifiedIdx != int(key):
                                config.remove_option(sectName, str(key))
                                config.set(sectName, str(newUnifiedIdx), value)
                        else:
                            config.remove_option(sectName, str(key))

            # rewrite the data back to the file.
            with open(filename, "w") as fd:

                fcntl.flock(fd, fcntl.LOCK_EX)
                config.write(fd)
                os.fsync(fd)
                fcntl.flock(fd, fcntl.LOCK_UN)
        except:
            return

    def updateExtIOFunc(self, oldExtIOConfigs, newExtIOConfigs):
        # oldExtIOConfigs: list of old ExtIO configs
        # newExtIOConfigs: list of new ExtIO configs
        # get current func io settings
        # iterate the extended IO pins,
        # get the new unified pin index for the function IO
        update_jkzuc_status()
        # extIONumIO模块数量
        # extIONum = s.extio_num

        currFuncDIMap = list(s.funcdi)
        # iterate the function of ext DI pins
        for idx in range(0, FUNCDI_NUM):
            currFuncDIMap[idx] = list(currFuncDIMap[idx])
            if currFuncDIMap[idx][0] == 2:
                if currFuncDIMap[idx][1] >= 0:  # valid extio
                    # get the (modIdx, pinIdxInMod) as per the oldExtIOConfigs
                    modPinIdx = self.extIOUnifiedIndex2ModIndex(
                        0, currFuncDIMap[idx][1], oldExtIOConfigs
                    )
                    newUnifiedIdx = self.extIOModIndex2UnifiedIndex(
                        0, modPinIdx, newExtIOConfigs
                    )
                    if newUnifiedIdx >= 0:
                        currFuncDIMap[idx] = [2, newUnifiedIdx]
                    else:
                        currFuncDIMap[idx] = [-1, -1]
                else:
                    currFuncDIMap[idx] = [-1, -1]

        c.set_funcdi(currFuncDIMap)

        # iterate the function of ext DO pins
        currFuncDOMap = list(s.funcdo)
        for actIdx in range(0, FUNCDO_NUM):
            currFuncDOMap[actIdx] = list(currFuncDOMap[actIdx])
            # remove the duplicated IO
            tempMap = {}.fromkeys(currFuncDOMap[actIdx]).keys()
            currFuncDOMap[actIdx] = [[-1, -1], [-1, -1], [-1, -1], [-1, -1]]
            currFuncDOMap[actIdx][0 : len(tempMap)] = tempMap
            for optIdx in range(len(currFuncDOMap[actIdx])):
                currFuncDOMap[actIdx][optIdx] = list(currFuncDOMap[actIdx][optIdx])
                if currFuncDOMap[actIdx][optIdx][0] == 2:  # extio
                    if currFuncDOMap[actIdx][optIdx][1] >= 0:  # valid index
                        modPinIdx = self.extIOUnifiedIndex2ModIndex(
                            1, currFuncDOMap[actIdx][optIdx][1], oldExtIOConfigs
                        )
                        newUnifiedIdx = self.extIOModIndex2UnifiedIndex(
                            1, modPinIdx, newExtIOConfigs
                        )
                        if newUnifiedIdx >= 0:
                            currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                        else:
                            currFuncDOMap[actIdx][optIdx] = [-1, -1]
                    else:
                        currFuncDOMap[actIdx][optIdx] = [-1, -1]

        c.set_funcdo(currFuncDOMap)

    def updateExtIOFunc_new(self, oldExtIOConfigs, newExtIOConfigs, modID):
        # oldExtIOConfigs: list of old ExtIO configs
        # newExtIOConfigs: list of new ExtIO configs
        # get current func io settings
        # iterate the extended IO pins,
        # get the new unified pin index for the function IO
        update_jkzuc_status()
        # extIONumIO模块数量
        # extIONum = s.extio_num
        currFuncDIMap = list(s.funcdi)
        # iterate the function of ext DI pins
        for idx in range(0, FUNCDI_NUM):
            currFuncDIMap[idx] = list(currFuncDIMap[idx])
            if currFuncDIMap[idx][0] == 2:
                if currFuncDIMap[idx][1] >= 0:  # valid extio
                    # get the (modIdx, pinIdxInMod) as per the oldExtIOConfigs
                    modPinIdx = self.extIOUnifiedIndex2ModIndex(
                        0, currFuncDIMap[idx][1], oldExtIOConfigs
                    )
                    mod, pinIdx = modPinIdx
                    if mod == modID:
                        newUnifiedIdx = -1
                        currFuncDIMap[idx] = [-1, newUnifiedIdx]
                    elif mod > modID:
                        newUnifiedIdx = int(currFuncDIMap[idx][1]) - int(
                            oldExtIOConfigs[1][modID][3][0][1]
                        )
                        currFuncDIMap[idx] = [2, newUnifiedIdx]
                    else:
                        newUnifiedIdx = int(currFuncDIMap[idx][1])
                        currFuncDIMap[idx] = [2, newUnifiedIdx]
                else:
                    currFuncDIMap[idx] = [-1, -1]

        c.set_funcdi(currFuncDIMap)

        currFuncDOMap = list(s.funcdo)

        for actIdx in range(0, FUNCDO_NUM):
            currFuncDOMap[actIdx] = list(currFuncDOMap[actIdx])
            # remove the duplicated IO
            tempMap = {}.fromkeys(currFuncDOMap[actIdx]).keys()
            currFuncDOMap[actIdx] = [[-1, -1], [-1, -1], [-1, -1], [-1, -1]]
            currFuncDOMap[actIdx][0 : len(tempMap)] = tempMap
            # grpc_logger().info("1.1currFuncDOMap[actIdx]: {}".format(currFuncDOMap[actIdx]))
            for optIdx in range(len(currFuncDOMap[actIdx])):
                currFuncDOMap[actIdx][optIdx] = list(currFuncDOMap[actIdx][optIdx])
                if currFuncDOMap[actIdx][optIdx][0] == 2:  # extio
                    if currFuncDOMap[actIdx][optIdx][1] >= 0:  # valid index
                        modPinIdx = self.extIOUnifiedIndex2ModIndex(
                            1, currFuncDOMap[actIdx][optIdx][1], oldExtIOConfigs
                        )
                        mod, pinIdx = modPinIdx
                        if mod == modID:
                            newUnifiedIdx = -1
                            currFuncDOMap[actIdx][optIdx] = [-1, newUnifiedIdx]
                        elif mod > modID:
                            newUnifiedIdx = int(currFuncDOMap[actIdx][optIdx][1]) - int(
                                oldExtIOConfigs[1][modID][3][1][1]
                            )
                            currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                        else:
                            newUnifiedIdx = int(currFuncDOMap[actIdx][optIdx][1])
                            currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                    else:
                        currFuncDOMap[actIdx][optIdx] = [-1, -1]

        c.set_funcdo(currFuncDOMap)

    def updateExtIOFunc_move(
        self, oldExtIOConfigs, newExtIOConfigs, currIndex, tarIndex
    ):
        # oldExtIOConfigs: list of old ExtIO configs
        # newExtIOConfigs: list of new ExtIO configs
        # get current func io settings
        # iterate the extended IO pins,
        # get the new unified pin index for the function IO
        update_jkzuc_status()
        # extIONumIO模块数量
        # extIONum = s.extio_num
        currFuncDIMap = list(s.funcdi)

        # iterate the function of ext DI pins
        for idx in range(0, FUNCDI_NUM):
            currFuncDIMap[idx] = list(currFuncDIMap[idx])
            if currFuncDIMap[idx][0] == 2:
                if currFuncDIMap[idx][1] >= 0:  # valid extio
                    # get the (modIdx, pinIdxInMod) as per the oldExtIOConfigs
                    modPinIdx = self.extIOUnifiedIndex2ModIndex(
                        0, currFuncDIMap[idx][1], oldExtIOConfigs
                    )
                    mod, pinIdx = modPinIdx
                    newMod = -1
                    key = currFuncDIMap[idx][1]
                    if currIndex > tarIndex:
                        if mod == currIndex:
                            newMod = mod - (currIndex - tarIndex)
                            ioNum = 0
                            for modNum in range(newMod + 1, currIndex + 1):
                                ioNum = ioNum + newExtIOConfigs[1][modNum][3][0][1]
                            newUnifiedIdx = int(key) - ioNum
                            currFuncDIMap[idx] = [2, newUnifiedIdx]
                        elif mod > currIndex:
                            newMod = mod
                            newUnifiedIdx = int(key)
                            currFuncDIMap[idx] = [2, newUnifiedIdx]
                        else:
                            if mod < tarIndex:
                                newMod = mod
                                newUnifiedIdx = int(key)
                                currFuncDIMap[idx] = [2, newUnifiedIdx]
                            else:
                                newMod = mod + 1
                                newUnifiedIdx = (
                                    int(key) + oldExtIOConfigs[1][currIndex][3][0][1]
                                )
                                currFuncDIMap[idx] = [2, newUnifiedIdx]

                    else:
                        # currIndex < tarIndex
                        if mod == currIndex:
                            newMod = mod + (tarIndex - currIndex)
                            ioNum = 0
                            for modNum in range(currIndex, newMod):
                                ioNum = ioNum + newExtIOConfigs[1][modNum][3][0][1]
                            newUnifiedIdx = int(key) + ioNum
                            currFuncDIMap[idx] = [2, newUnifiedIdx]
                        elif mod < currIndex:
                            newMod = mod
                            newUnifiedIdx = int(key)
                            currFuncDIMap[idx] = [2, newUnifiedIdx]
                        else:
                            if mod <= tarIndex:
                                newMod = mod - 1
                                newUnifiedIdx = (
                                    int(key) - oldExtIOConfigs[1][currIndex][3][0][1]
                                )
                                currFuncDIMap[idx] = [2, newUnifiedIdx]

                            else:
                                newMod = mod
                                newUnifiedIdx = int(key)
                                currFuncDIMap[idx] = [2, newUnifiedIdx]

                else:
                    currFuncDIMap[idx] = [-1, -1]

        c.set_funcdi(currFuncDIMap)

        # iterate the function of ext DO pins
        currFuncDOMap = list(s.funcdo)

        for actIdx in range(0, FUNCDO_NUM):
            currFuncDOMap[actIdx] = list(currFuncDOMap[actIdx])
            tempMap = {}.fromkeys(currFuncDOMap[actIdx]).keys()
            currFuncDOMap[actIdx] = [[-1, -1], [-1, -1], [-1, -1], [-1, -1]]
            currFuncDOMap[actIdx][0 : len(tempMap)] = tempMap
            for optIdx in range(len(currFuncDOMap[actIdx])):
                currFuncDOMap[actIdx][optIdx] = list(currFuncDOMap[actIdx][optIdx])
                if currFuncDOMap[actIdx][optIdx][0] == 2:  # extio
                    if currFuncDOMap[actIdx][optIdx][1] >= 0:  # valid index
                        modPinIdx = self.extIOUnifiedIndex2ModIndex(
                            1, currFuncDOMap[actIdx][optIdx][1], oldExtIOConfigs
                        )
                        mod, pinIdx = modPinIdx
                        newMod = -1
                        key = currFuncDOMap[actIdx][optIdx][1]
                        if currIndex > tarIndex:
                            grpc_logger().info("currIndex > tarIndex:")
                            if mod == currIndex:
                                newMod = mod - (currIndex - tarIndex)
                                ioNum = 0
                                for modNum in range(newMod + 1, currIndex + 1):
                                    ioNum = ioNum + newExtIOConfigs[0][modNum][3][1][1]
                                newUnifiedIdx = int(key) - ioNum
                                currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                            elif mod > currIndex:
                                newMod = mod
                                newUnifiedIdx = int(key)
                                currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                            else:
                                if mod < tarIndex:
                                    newMod = mod
                                    newUnifiedIdx = int(key)
                                    currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                                else:
                                    newMod = mod + 1
                                    newUnifiedIdx = (
                                        int(key)
                                        + oldExtIOConfigs[1][currIndex][3][1][1]
                                    )
                                    currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                        else:
                            # currIndex < tarIndex
                            if mod == currIndex:
                                newMod = mod + (tarIndex - currIndex)
                                ioNum = 0
                                for modNum in range(currIndex, newMod):
                                    ioNum = ioNum + newExtIOConfigs[1][modNum][3][1][1]
                                newUnifiedIdx = int(key) + ioNum
                                currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                            elif mod < currIndex:
                                newMod = mod
                                newUnifiedIdx = int(key)
                                currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                            else:
                                if mod <= tarIndex:
                                    newMod = mod - 1
                                    newUnifiedIdx = (
                                        int(key)
                                        - oldExtIOConfigs[1][currIndex][3][1][1]
                                    )
                                    currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                                else:
                                    newMod = mod
                                    newUnifiedIdx = int(key)
                                    currFuncDOMap[actIdx][optIdx] = [2, newUnifiedIdx]
                    else:
                        currFuncDOMap[actIdx][optIdx] = [-1, -1]

        c.set_funcdo(currFuncDOMap)

    def save_config(self, filename, kw):
        # 写配置文件:kw = {section1：{key:value,key:value},section2:{key:value,key:value}}
        try:
            config = MyConfigParser()
            config.read(filename)
            for section, parameter in kw.items():
                for key, value in parameter.items():
                    if not config.has_section(section):
                        config.add_section(section)
                    config.set(section, key, str(value))
            with open(filename, "w") as fd:
                # fcntl.flock(fd, fcntl.LOCK_EX)
                config.write(fd)
                # os.fsync(fd)
                # fcntl.flock(fd, fcntl.LOCK_UN)
        except Exception:
            grpc_logger().error("ERROR|save_config: {}".format(traceback.format_exc()))

    # 读配置文件:kw = {section1：{key1:"",key2:""},section2:{key3:"",key4:""}}
    def read_config(self, filename, kw):
        dict = {}
        try:
            config = MyConfigParser()
            config.read(filename)
            for section, parameter in kw.items():
                if config.has_section(section):
                    dict[section] = {}
                    for key, value in parameter.items():
                        if config.has_option(section, key):
                            dict[section][key] = config.get(section, key)
                        else:
                            dict[section][key] = value
            return dict
        except Exception:
            grpc_logger().error("ERROR|read_config: {}".format(traceback.format_exc()))
            return dict

    def timer_shutdown(self):
        grpc_logger().debug("[gRPC] timer shutdown")
        try:
            tool_function_ctx = tool_function.ModuleCtx(grpc_logger)
            tool_function.runCmdAsRoot("shutdown", tool_function_ctx)
        except Exception:
            grpc_logger().debug("[gRPC] error shutdown")

    def ensure_mode(self, m, *p):
        update_jkzuc_status()
        if s.task_mode == m or s.task_mode in p:
            return True
        c.mode(m)  # task_mode
        c.wait_complete()
        update_jkzuc_status()
        return True

    def check_extio_constraint(
        self, mod_index, req_di_num, req_do_num, req_ai_num, req_ao_num, is_add_extmod
    ):
        update_jkzuc_status()
        current_extmod_num = s.extio_num
        extio_mod_num = 1 if is_add_extmod else 0

        if not is_add_extmod:
            current_di_num = len(s.extio_stat[0]) - s.extio_setup[mod_index][3][0][1]
            current_do_num = len(s.extio_stat[1]) - s.extio_setup[mod_index][3][1][1]
            current_ai_num = len(s.extio_stat[2]) - s.extio_setup[mod_index][3][2][1]
            current_ao_num = len(s.extio_stat[3]) - s.extio_setup[mod_index][3][3][1]
        else:
            current_di_num = len(s.extio_stat[0])
            current_do_num = len(s.extio_stat[1])
            current_ai_num = len(s.extio_stat[2])
            current_ao_num = len(s.extio_stat[3])

        if current_extmod_num + extio_mod_num > MAX_EXTIO_CNT:
            return False
        elif req_di_num + current_di_num > MAX_EXTIO_DI_NUM:
            return False
        elif req_do_num + current_do_num > MAX_EXTIO_DO_NUM:
            return False
        elif req_ai_num + current_ai_num > MAX_EXTIO_AI_NUM:
            return False
        elif req_ao_num + current_ao_num > MAX_EXTIO_AO_NUM:
            return False
        else:
            return True

    ## servo upgrade monitor in background
    #
    #  monitor the progress of servo upgrading, reboot
    #  right after upgrading fails or complete
    def servo_upgrade_monitor(self):
        update_jkzuc_status()
        while s.servo_upGrade_percentage_total < 100:
            time.sleep(1)
            update_jkzuc_status()
        time.sleep(3)
        timer_reboot()

    def is_program_folder(self, folder_path):
        innerFiles = os.listdir(folder_path)
        for k in range(len(innerFiles)):
            try:
                innerFiles[k] = os.path.splitext(innerFiles[k])[1]
            except:
                innerFiles[k] = ""
        if ".zu" in innerFiles:
            return True
        else:
            return False

    def sync_path(self, path):
        libc = ctypes.CDLL("libc.so.6")
        if os.path.isfile(path):
            fd = os.open(path, os.O_RDWR)
            libc.fsync(fd)
            os.close(fd)
        elif os.path.isdir(path):
            file_list = os.listdir(path)
            for file_name in file_list:
                file_path = os.path.join(path, file_name)
                if os.path.islink(file_path):
                    grpc_logger().debug(
                        "[gRPC] file link {} is jumped.".format(file_path)
                    )
                elif os.path.isfile(file_path):
                    fd = os.open(file_path, os.O_RDWR)
                    libc.fsync(fd)
                    os.close(fd)
                    grpc_logger().debug(
                        "[gRPC] {} is synced to disk, size: {}".format(
                            file_path, os.path.getsize(file_path)
                        )
                    )
                elif os.path.isdir(file_path):
                    self.sync_path(file_path)
                    grpc_logger().debug(
                        "[gRPC] subdir {} is synced to disk".format(file_path)
                    )

    def loadSystemVars(self):
        try:
            redis_conn.delete("scriptVariables")
            filename = jkzuc.jkroot_dir + "/configs/JAKA/jaka_user.var"
            with open(filename, "r+") as fd:
                for line in fd.readlines():
                    line = line.rstrip()
                    item_list = line.split()
                    item_dict = {}
                    item_dict[item_list[0]] = "{:g}".format(float(item_list[1]))
                    redis_conn.hmset("scriptVariables", item_dict)
                    del item_dict
        except redis.exceptions.ConnectionError:
            grpc_logger().debug("[gRPC] memory server is not running")
        except Exception:
            grpc_logger().error(traceback.format_exc())

    ##########################################################################################
    ################################# grpc server interfaces #################################
    ##########################################################################################

    def SetAdmittanceDragDeadZone(self, request, context):

        def CheckParam(cartpos):
            return (
                (0.5 <= cartpos.x <= 100.0)
                and (0.5 <= cartpos.y <= 100.0)
                and (0.5 <= cartpos.z <= 100.0)
                and (0.05 <= cartpos.a <= 5.0)
                and (0.05 <= cartpos.b <= 5.0)
                and (0.05 <= cartpos.c <= 5.0)
            )

        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            cartpos = request.cartpos
            if not CheckParam(cartpos):
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
                )
            c.set_admittance_drag_dead_zone(
                cartpos.x, cartpos.y, cartpos.z, cartpos.a, cartpos.b, cartpos.c
            )
            return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK)
        except Exception:
            grpc_logger().error(traceback.format_exc())

    def GetAdmittanceDragDeadZone(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            drag_dead_zone = s.drag_dead_zone
            cartpos = jaka_pb2.CartPos()
            cartpos.x = drag_dead_zone[0]
            cartpos.y = drag_dead_zone[1]
            cartpos.z = drag_dead_zone[2]
            cartpos.a = drag_dead_zone[3]
            cartpos.b = drag_dead_zone[4]
            cartpos.c = drag_dead_zone[5]
            return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK, cartpos=cartpos)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
            )

    def GetCabVoltage(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            vol = s.cur_cab_voltage
            return jaka_pb2.Response(errcode=0, cabVoltage=vol)
        except Exception:
            grpc_logger().debug("[gRPC] login exception")
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetCabVoltage(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.setCabVoltage(request.cabVoltage)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().debug("[gRPC] login exception")
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def LogIn(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if self.LOGSTATUS:
                grpc_logger().debug("[gRPC] user already logged-in")
                # 用于返回状态给APP 判断点击重连时返回的重复连接是被其他设备连接还是当前APP掉线但是控制器还没断连导致的重复连接
                client_ip = context.peer().split(":")[1]
                if client_ip != self.clientIP:
                    return jaka_pb2.Response(
                        errcode=4, errormsg="Login failed:Repeat login", intid=0
                    )
                else:
                    return jaka_pb2.Response(
                        errcode=4, errormsg="Login failed:Repeat login", intid=1
                    )

            file_name = jkzuc.jkroot_dir + "/configs/JAKA/jakar7_user.ini"
            loginfo = request.loginfo
            user_info = {"username": "", "password": ""}
            kw = {
                "administrator": user_info,
                "operator": user_info,
                "visitor": user_info,
            }
            user_dict = self.read_config(file_name, kw)

            # 对密码md5加密
            # grpc_logger().debug("loginfo.password: {}".format(loginfo.password))
            md5_password = tool_function.getMD5(loginfo.password)
            # 匹配用户名和密码是否有效
            for section in user_dict:
                if loginfo.username == user_dict[section]["username"]:
                    tool_function_ctx = tool_function.ModuleCtx(grpc_logger)
                    self.password = tool_function.read_ini_config(
                        file_name, section, "password", module_ctx=tool_function_ctx
                    )
                    if (not self.password) or (md5_password != self.password):
                        grpc_logger().debug(
                            "[gRPC] login failed due to incorrect password"
                        )
                        return jaka_pb2.Response(
                            errcode=2, errormsg="Login failed:incorrect password"
                        )
                    if "127.0.0.1" != context.peer().split(":")[1]:
                        Robot.client_uq_IPA = context.peer().split(":")[1]
                        grpc_logger().debug(
                            "[grpc] A new client ,IP Address: {}".format(
                                Robot.client_uq_IPA
                            )
                        )
                    peer_Info = context.peer().split(":")
                    self.clientIP = peer_Info[1]
                    self.clientPort = peer_Info[2]
                    grpc_logger().debug("peer info: {}".format(peer_Info))
                    self.LOGSTATUS = True
                    self.username = loginfo.username
                    self.userlevel = section
                    break

            if loginfo.username != self.username:
                grpc_logger().debug("[gRPC] login failed due to incorrect username")
                return jaka_pb2.Response(
                    errcode=3, errormsg="Login failed:incorrect username"
                )

            global HEARTBEAT
            global CONNECT_FLAG
            global CURRENT_TIME
            CONNECT_FLAG = True

            # fix: 通过AddOn登录时，设置CONNECT_FLAG为True，由于HEARTBEAT为False，所以导致CONNECT_FLAG不能被设回False;
            # 导致UDP广播消息为控制器已被连接，APP无法连连接控制器
            HEARTBEAT = True

            # fix: 同上，CURRENT_TIME为0时，也不能将CONNECT_FLAG设置为False
            CURRENT_TIME = time.time()

            # grpc_logger().debug("[GRPC] connect_flag true")
            c.set_app_connection_status(1)

            # expired_time = datetime(2023, 9, 30, 0, 0)
            # now_time = datetime.now()
            # if now_time < expired_time:
            #     grpc_logger().debug("[gRPC] TEST VERSION LOGIN PASS")
            # else:
            #     grpc_logger().debug("[gRPC] TEST VERSION EXPIRED")
            #     c.set_grpc_errcode(jkzuc.OPERATOR_ERROR,17761025, "Software trial has expired",0xff,0)
            grpc_logger().debug("[gRPC] user logged in")
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().debug("[gRPC] login exception")
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def LogOut(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            loginfo = request.loginfo
            if loginfo.username != self.username:
                return jaka_pb2.Response(
                    errcode=3, errormsg="Logout failed:incorrect username"
                )

            md5_password = tool_function.getMD5(loginfo.password)
            if md5_password != self.password:
                return jaka_pb2.Response(
                    errcode=2, errormsg="Logout failed:incorrect password"
                )

            self.LOGSTATUS = False
            Robot.client_uq_IPA = ""
            grpc_logger().info("[TEST] CLEAR IPA {}".format(Robot.client_uq_IPA))
            self.username = ""
            self.password = ""
            self.userlevel = ""
            global CONNECT_FLAG
            CONNECT_FLAG = False
            grpc_logger().info("[GRPC] connect_flag FALSE")
            c.set_app_connection_status(0)
            grpc_logger().debug("[gRPC] user logged out")
            return jaka_pb2.Response(errcode=0)
        except Exception:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def RobotStateChange(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            reqstat = request.statechangereq.expstate
            if(reqstat == jaka_pb2.StateChangeReq.STATE_POWER_ON):
                c.scb_power_on()
            elif(reqstat == jaka_pb2.StateChangeReq.STATE_POWER_OFF):
                c.scb_power_off()
            elif(reqstat == jaka_pb2.StateChangeReq.STATE_ENABLE):
                c.rob_enable()
            elif(reqstat == jaka_pb2.StateChangeReq.STATE_DISABLE):
                c.rob_disable()
            else:
                return jaka_pb2.Response(errcode=jaka_pb2.RPC_INVALID_ARGS, errormsg="invalid statechangereq parameter")
            
            grpc_logger().debug(
                "[JakaDebug] cab_type:{}, controller version:{},servo_version:{}, scb_version:{} ".format(
                    s.cab_type,
                    jkzuc.controller_version,
                    tool_function.getServoVerStr(s.servo_version),
                    tool_function.getScbVerStr(s),
                )
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ModeChange(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            mode_change = request.modechangereq
            self.ensure_mode(mode_change.expmode)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def Jog(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            jogcoord = 1  # joint
            # self.ensure_mode(jkzuc.MODE_MANUAL)
            if request.jogreq.jogcoord == jaka_pb2.JogReq.JOG_JOINT:
                jogcoord = 1  # joint
            elif request.jogreq.jogcoord == jaka_pb2.JogReq.JOG_WORLD:
                jogcoord = 0  # world
            elif request.jogreq.jogcoord == jaka_pb2.JogReq.JOG_TOOL:
                jogcoord = 2  # tool
            else:
                return jaka_pb2.Response(errcode=22, errormsg="unsupported jog coord")

            if 1: #s.robot(request.robot_index).inpos: 由于当前motion会在1s内自动停止jog，所以需要持续发送
                if request.jogreq.jogmode == jaka_pb2.JogReq.JOG_ABS:
                    grpc_logger().debug(
                        "[gRPC] rob{:d} joint/axis {:d} jog abs called".format(
                            request.robot_index,
                            request.jogreq.ajnum
                        )
                    )
                    c.robot_jog(
                        request.robot_index,
                        jkzuc.JOG_ABS,
                        jogcoord,
                        request.jogreq.ajnum,
                        request.jogreq.jogvel,
                        request.jogreq.poscmd,
                    )
                    return jaka_pb2.Response(errcode=0)
                elif request.jogreq.jogmode == jaka_pb2.JogReq.JOG_CONT:
                    grpc_logger().debug(
                        "[gRPC] rob{:d} joint/axis {:d} jog cont called".format(
                            request.robot_index,
                            request.jogreq.ajnum
                        )
                    )
                    c.robot_jog(
                        request.robot_index,
                        jkzuc.JOG_CONTINUOUS,
                        jogcoord,
                        request.jogreq.ajnum,
                        request.jogreq.jogvel,
                    )
                    return jaka_pb2.Response(errcode=0)
                elif request.jogreq.jogmode == jaka_pb2.JogReq.JOG_INCR:
                    grpc_logger().debug(
                        "[gRPC] rob{:d} joint/axis {:d} jog incr called".format(                            
                            request.robot_index,
                            request.jogreq.ajnum
                        )
                    )
                    c.robot_jog(
                        request.robot_index,
                        jkzuc.JOG_INCREMENT,
                        jogcoord,
                        request.jogreq.ajnum,
                        request.jogreq.jogvel,
                        request.jogreq.poscmd,
                    )
                    return jaka_pb2.Response(errcode=0)
                else:
                    return jaka_pb2.Response(
                        errcode=23, errormsg="unsupported jog mode"
                    )
            else:
                return jaka_pb2.Response(errcode=24, errormsg="robot are moving ")
        except Exception:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def JogMovJ(self, request, context):
        '''
            关节运动至指定位置
        '''
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            movj_req = request.movjreq
            robot_index = request.robot_index
            update_jkzuc_status()
            if s.enabled and s.robot(request.robot_index).inpos:
                # c.mode(jkzuc.MODE_MANUAL)  # for queue clear
                # self.ensure_mode(jkzuc.MODE_AUTO)
                c.robot_jog_multi_joint(
                    robot_index,
                    movj_req.velocity,
                    [
                        movj_req.jointpos.joint1,
                        movj_req.jointpos.joint2,
                        movj_req.jointpos.joint3,
                        movj_req.jointpos.joint4,
                        movj_req.jointpos.joint5,
                        movj_req.jointpos.joint6,
                        movj_req.jointpos.joint7
                    ]
                )
                
                grpc_logger().debug("[gRPC] jog_movj called")
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def JogMovL(self, request, context):
        '''
            直线运动至指定位置
        '''
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            movl_req = request.movlreq
            update_jkzuc_status()
            if s.enabled and s.robot(request.robot_index).inpos:
                # c.teleop_enable(0)  # for queue clear
                # self.ensure_mode(jkzuc.MODE_AUTO)
                c.jog_movl(
                    request.robot_index,
                    0x10,                    
                    (movl_req.cartpos.x,
                    movl_req.cartpos.y,
                    movl_req.cartpos.z,
                    movl_req.cartpos.a,
                    movl_req.cartpos.b,
                    movl_req.cartpos.c),
                    movl_req.velocity,
                    500.0,
                )
                grpc_logger().debug("[gRPC] jog_movl called")
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetDout(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            dout = request.dout
            c.set_digital_output(dout.type, dout.index, int(dout.value))
            grpc_logger().debug(
                "[gRPC] set dout {:d}:{:d}:{:d}".format(
                    dout.type, dout.index, int(dout.value)
                )
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetAout(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            aout = request.aout
            c.set_analog_output(aout.type, aout.index, aout.value)
            grpc_logger().debug(
                "[gRPC] set aout {:d}:{:d}:{:d}".format(
                    aout.type, aout.index, int(aout.value)
                )
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def MtnAbort(self, request, context):
        """
        该接口不用于jog功能的停止
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        # 因为jog过程中,mtnabort调用本身可能执行时间会很长，所以采用子线程的方式运行，以避免app报断线
        mtnProcess = threading.Thread(target=mtnAbortProcess)
        mtnProcess.start()
        mtnProcess.join(0.1)
        return jaka_pb2.Response(errcode=0)

    def Home(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            for jnum in range(0, s.robot_joint_num):
                if not (s.homed[jnum]):
                    c.teleop_enable(0)
                    time.sleep(0.05)
                    # c.home(-1)
                    time.sleep(0.05)
                    break
            return jaka_pb2.Response(errcode=0)
        except Exception:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    @sim_mode_required(0)
    def ServoHome(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            c.servo_home(request.intid)
            grpc_logger().debug("[gRPC] servo home joint{:d}".format(request.intid))
            return jaka_pb2.Response(errcode=0)
        except Exception:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def JogStop(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            # if self.LOGSTATUS:
            update_jkzuc_status()

            if s.enabled:
                grpc_logger().debug("[gRPC] all robot jog stop")
                # FIXME：此处先清除错误，后续改为app提示，然后再触发清除错误
                c.robot_clear_error(0)
                c.robot_clear_error(1)
                c.abort()

            return jaka_pb2.Response(errcode=0)
        except Exception:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def HeartBeatErrCode(self, heartbeatData, isAddonProxy):
        if isAddonProxy:  # the 4th bit for AddonProxy repeat
            sendBit = 0x10
        else:  # the 1st bit for GRPC repeat
            sendBit = 0x1

        error_stat = e.poll()
        if error_stat:
            kind, errcode, text = error_stat
            if kind in (jkzuc.NML_ERROR, jkzuc.OPERATOR_ERROR):
                # heartbeatData.errtype = -1
                heartbeatData.errcode = errcode
                heartbeatData.errstring = text
            elif kind in (jkzuc.OPERATOR_DISPLAY, jkzuc.OPERATOR_TEXT):
                heartbeatData.errtype = 1
                heartbeatData.errcode = errcode
                heartbeatData.errstring = text
            if errcode != 0:
                repeatflag = (~sendBit) & 0xFF
                c.set_grpc_errcode(int(kind), int(errcode), str(text), repeatflag, 0)
        else:
            kind, errcode, text, repeatflag = s.grpc_errcode
            if (
                errcode != 0 and repeatflag & sendBit
            ):  # jungle the 1st bit of repeatflag if 1
                repeatflag = repeatflag & ((~sendBit) & 0xFF)
                c.set_grpc_errcode(int(kind), int(errcode), str(text), repeatflag, 0)
                if kind in (
                    jkzuc.NML_ERROR,
                    jkzuc.OPERATOR_ERROR,
                    jkzuc.GRPC_RESPONSE_ERROR,
                    jkzuc.GRPC_RESPONSE_WARNING,
                ):
                    heartbeatData.errtype = -1
                    heartbeatData.errcode = errcode
                    heartbeatData.errstring = text
                elif kind in (
                    jkzuc.OPERATOR_TEXT,
                    jkzuc.OPERATOR_DISPLAY,
                    jkzuc.GRPC_RESPONSE_INFO,
                ):
                    heartbeatData.errtype = 1
                    heartbeatData.errcode = errcode
                    heartbeatData.errstring = text
            else:
                heartbeatData.errtype = 0
                heartbeatData.errcode = 0
                heartbeatData.errstring = ""

    def GetRobotStat(self, robindex:int) -> jaka_pb2.RobotStat:
        '''
            获取robindex指定的机器人状态
        '''
        robotStat = jaka_pb2.RobotStat()
        rb = s.robot(robindex)
        for i in range(len(rb.cmd_jpos)):
            robotStat.cmd_jpos.append(rb.cmd_jpos[i])
            robotStat.act_jpos.append(rb.act_jpos[i])
        
        robotStat.cmd_pose.x = rb.cmd_pose[0]
        robotStat.cmd_pose.y = rb.cmd_pose[1]
        robotStat.cmd_pose.z = rb.cmd_pose[2]
        robotStat.cmd_pose.a = rb.cmd_pose[3]
        robotStat.cmd_pose.b = rb.cmd_pose[4]
        robotStat.cmd_pose.c = rb.cmd_pose[5]

        robotStat.act_pose.x = rb.act_pose[0]
        robotStat.act_pose.y = rb.act_pose[1]
        robotStat.act_pose.z = rb.act_pose[2]
        robotStat.act_pose.a = rb.act_pose[3]
        robotStat.act_pose.b = rb.act_pose[4]
        robotStat.act_pose.c = rb.act_pose[5]

        robotStat.powered_on = rb.powered_on
        robotStat.enabled = rb.enabled
        robotStat.in_drag = 0
        robotStat.is_error = 0
        
        for i in range(rb.model_data["jointnum"]):
            robotStat.drag_near_limit.append(0)

        robotStat.on_slimit = 0
        robotStat.protective_stop = 0
        robotStat.reduce_mode = 0

        robotStat.tool_id = rb.tool_id
        robotStat.userframe_id = rb.usrframe_id
        robotStat.baseframe_id = rb.baseframe_id
        robotStat.payload_id = rb.payload_id

        robotStat.is_backdrive = 0
        robotStat.is_moving = 0
        robotStat.serialnum_str = ""

        robotStat.rob_index = robindex
      
        return robotStat
    
    def HeartBeat(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        global JOG_TIME
        try:
            isAddonProxy = True
            client_ip = context.peer().split(":")[1]
            if "127.0.0.1" != client_ip:  # if not loop client(addon)
                # JOG_TIME = time.time()
                isAddonProxy = False

            update_jkzuc_status()
            heartbeatData = jaka_pb2.HeartBeat()

            # 更新机械臂末端灯颜色
            heartbeatData.tio_lamp_color = s.tio_lamp_color

            # 心跳错误码防止重复发送机制
            self.HeartBeatErrCode(heartbeatData, isAddonProxy)

            # 关节空间下角度,return commanded as actual
            temp_position = s.joint_position
            heartbeatData.joint_position.joint1 = temp_position[0]
            heartbeatData.joint_position.joint2 = temp_position[1]
            heartbeatData.joint_position.joint3 = temp_position[2]
            heartbeatData.joint_position.joint4 = temp_position[3]
            heartbeatData.joint_position.joint5 = temp_position[4]
            heartbeatData.joint_position.joint6 = temp_position[5]
            heartbeatData.joint_position.joint7 = temp_position[6]

            # 笛卡尔空间下角度
            temp_position = s.position
            heartbeatData.cartesian_position.x = temp_position[0]
            heartbeatData.cartesian_position.y = temp_position[1]
            heartbeatData.cartesian_position.z = temp_position[2]
            heartbeatData.cartesian_position.a = temp_position[3]
            heartbeatData.cartesian_position.b = temp_position[4]
            heartbeatData.cartesian_position.c = temp_position[5]

            # ngc脚本解析状态,机器使能,电源开关,ngc程序暂停,保护性停止,到达软限位,硬急停
            heartbeatData.robot_enabled = s.enabled
            heartbeatData.robot_powered_on = s.powered_on
            heartbeatData.IsPoweringOn = s.is_powering_on
            heartbeatData.IsBeingEnabled = s.is_being_enabled

            heartbeatData.robot_in_drag = s.motion_mode == jkzuc.TRAJ_MODE_DRAG
            heartbeatData.robot_on_slimit = s.on_soft_limit

            heartbeatData.isSteppingMode = s.is_stepping_mode
            heartbeatData.stepping_state = s.stepping_state

            if s.interp_state == jkzuc.INTERP_IDLE:
                heartbeatData.prog_stat = jaka_pb2.PROG_IDLE
            elif s.interp_state == jkzuc.INTERP_PAUSED or s.task_paused:
                heartbeatData.prog_stat = jaka_pb2.PROG_PAUSED
            else:
                heartbeatData.prog_stat = jaka_pb2.PROG_RUNNING

            # 缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容grpc接口相关不做名称变更
            if s.percentage_mode_level == 3:  # level == 3 处于保护性停止模式
                heartbeatData.reduce_mode = jaka_pb2.REDUCE_STOP
            elif s.reduce_mode == 1:  # reduce mode == 1 处于缩减模式
                heartbeatData.reduce_mode = jaka_pb2.REDUCE
            elif s.percentage_mode_level == 2:  # level == 2 处于二级倍率模式
                heartbeatData.reduce_mode = jaka_pb2.REDUCE_LEVEL_TWO
            elif s.percentage_mode_level == 1:  # level == 1 处于一级倍率模式
                heartbeatData.reduce_mode = jaka_pb2.REDUCE_LEVEL_ONE
            elif s.percentage_mode_level == 0:  # level == 0 没有处于倍率模式
                heartbeatData.reduce_mode = jaka_pb2.REDUCE_DISABLED

            heartbeatData.protective_stop = s.protective_stop
            heartbeatData.emergency_stop = s.emergency_stop
            if self.oneUpgrade.isRunning():
                if self.oneUpgrade.getState() == OnekeyUpgrade.UpgradeState.ERROR:
                    c.set_grpc_errcode(
                        jkzuc.OPERATOR_ERROR, self.oneUpgrade.getError(), "", 0xFF, 0
                    )
                    grpc_logger().error(
                        "{} onekey timeout".format(self.oneUpgrade.getError())
                    )
                    self.oneUpgrade.reset()
                    self.oneUpgrade.setState(self.oneUpgrade.UpgradeState.DONE)
                heartbeatData.servo_upGrade_percentage_total = (
                    self.oneUpgrade.getProgress()
                )
            else:
                heartbeatData.servo_upGrade_percentage_total = (
                    s.servo_upGrade_percentage_total
                )
            heartbeatData.servo_upGrade_percentage_axis = (
                s.servo_upGrade_percentage_axis
            )

            # 反向驱动状态位
            heartbeatData.backdrive_state = s.backdrive_state

            global GET_EIP_ENABLE

            # NOTE: DO NOT read or write IO in hearbeat!! We didn't maintenance IO data here anymore.
            # please use GetIOCofingInfo() and GetIOState() instead.
            # IO数据
            for item in s.din:
                heartbeatData.din.append(item)
            for item in s.dout:
                heartbeatData.dout.append(item)
            for item in s.ain:
                heartbeatData.ain.append(item)
            for item in s.aout:
                heartbeatData.aout.append(item)
            for item in s.tio_din:
                heartbeatData.tio_din.append(item)
            for item in s.tio_dout:
                heartbeatData.tio_dout.append(item)
            for item in s.tio_ain:
                heartbeatData.tio_ain.append(item)
            for item in s.tio_aout:
                heartbeatData.tio_aout.append(item)

            # point key state on tio
            # tio_button
            if s.point_button_opt[0]:
                heartbeatData.tio_key_state.pointState = s.tio_uikey[1]
            else:
                heartbeatData.tio_key_state.pointState = False

            # extio
            for item in s.extio_stat[0]:
                heartbeatData.extio_din.append(item)
            for item in s.extio_stat[1]:
                heartbeatData.extio_dout.append(item)
            for item in s.extio_stat[2]:
                heartbeatData.extio_ain.append(item)
            for item in s.extio_stat[3]:
                heartbeatData.extio_aout.append(item)
            heartbeatData.extio_state = s.extio_mode

            for item in s.drag_near_limit:
                heartbeatData.drag_near_limit.append(item)

            # torque sensor status
            torqSnsrStat = s.torqsensor[1]
            heartbeatData.torque_sensor_state = torqSnsrStat[0]
            del torqSnsrStat

            # admittance control
            heartbeatData.admittance_enabled = s.admittance_enabled

            # current tool/ucs
            heartbeatData.currToolId = s.current_tool_id
            heartbeatData.currUsrFrameId = s.current_user_id
            if s.cab_type >= 2:
                heartbeatData.scb_stick_locked = s.scb_stick_locked
            # stepping wait flag
            heartbeatData.steppingWait = s.stepping_wait
            heartbeatData.isSamplingTrajPoint = c.get_traj_sample_status()
            heartbeatData.isCollectingDiagData = s.is_diag_collecting
            heartbeatData.servoDebugEnable = self.servoDebugMode
            heartbeatData.safetySettingCRC = s.safety_crc

            # profinet state
            heartbeatData.profinetState = s.profinet_state

            # ethernet/IP state
            if GET_EIP_ENABLE == 0:
                heartbeatData.eip_state = 0  # EIP disable
            else:
                heartbeatData.eip_state = s.eip_state

            # current planned position
            temp_position = s.position_seg_target
            heartbeatData.position_segTarget.x = temp_position[0]
            heartbeatData.position_segTarget.y = temp_position[1]
            heartbeatData.position_segTarget.z = temp_position[2]
            heartbeatData.position_segTarget.a = temp_position[3]
            heartbeatData.position_segTarget.b = temp_position[4]
            heartbeatData.position_segTarget.c = temp_position[5]

            # asynchronous addon's installation status
            res, progress = addon_controller_inst.get_install_status()
            heartbeatData.addon_install_progress = progress
            addon_errcode = self.GetGrpcErrcodeFromAddon(res)
            heartbeatData.addon_install_status = addon_errcode

            # non-zero if robot is in postion
            heartbeatData.robot_inpos = s.inpos
            
            # 多机器人状态
            for i in range(2):
                robotStat = self.GetRobotStat(i)
                heartbeatData.robotStat.append(robotStat)
            self.hbRes.heartbeat.CopyFrom(heartbeatData)
            del heartbeatData
            return self.hbRes
        except MiddlewareException:
            # 伪造一个心跳
            heartbeatData = jaka_pb2.HeartBeat()
            heartbeatData.errtype = 2
            heartbeatData.errcode = 0x0FFFFE
            heartbeatData.errstring = "controller system err"
            # 需要手动填充 结构 数据,否则app识别不了
            heartbeatData.joint_position.joint1 = 0
            heartbeatData.joint_position.joint2 = 0
            heartbeatData.joint_position.joint3 = 0
            heartbeatData.joint_position.joint4 = 0
            heartbeatData.joint_position.joint5 = 0
            heartbeatData.joint_position.joint6 = 0
            heartbeatData.cartesian_position.x = 0
            heartbeatData.cartesian_position.y = 0
            heartbeatData.cartesian_position.z = 0
            heartbeatData.cartesian_position.a = 0
            heartbeatData.cartesian_position.b = 0
            heartbeatData.cartesian_position.c = 0
            heartbeatData.din.append(0)
            heartbeatData.dout.append(0)
            heartbeatData.ain.append(0)
            heartbeatData.aout.append(0)

            self.hbRes.heartbeat.CopyFrom(heartbeatData)
            del heartbeatData
            return self.hbRes
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTcp(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTcp(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ProgRun(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.teleop_enable(0)
            time.sleep(0.05)
            update_jkzuc_status()
            for jnum in range(0, s.robot_joint_num):
                if not (s.homed[jnum]):
                    # c.home(-1)
                    time.sleep(0.05)
                    break

            # clear redis database and load system variables
            self.loadSystemVars()
            # open the file
            c.task_plan_synch()
            time.sleep(0.05)
            scripts_path = jkzuc.jkroot_dir + "/scripts/"
            filePath = scripts_path + request.str
            real_path = os.path.realpath(filePath)
            if (scripts_path in real_path) == False:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_PERMISSION_DENY, errormsg="permission denied"
                )
            c.program_open(filePath)
            # ensure the mode
            update_jkzuc_status()
            self.ensure_mode(jkzuc.MODE_AUTO)
            startLine = request.intid
            if startLine <= 0:
                startLine = 0
            c.auto(jkzuc.AUTO_RUN, startLine)
            grpc_logger().debug("[gRPC] program run called")
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    # load a program
    def LoadProgram(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            if s.interp_state != jkzuc.INTERP_IDLE:
                return jaka_pb2.Response(
                    errcode=25, errormsg="Cannot load program when a program is running"
                )

            c.teleop_enable(0)
            time.sleep(0.05)

            c.task_plan_synch()
            time.sleep(0.05)
            scripts_path = jkzuc.jkroot_dir + "/scripts/"
            filePath = scripts_path + request.str
            real_path = os.path.realpath(filePath)
            if (scripts_path in real_path) == False:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_PERMISSION_DENY, errormsg="permission denied"
                )
            c.program_open(filePath)
            grpc_logger().debug("[gRPC] program loaded: {}".format(filePath))
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ProgPause(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if s.task_mode != jkzuc.MODE_AUTO:
                return jaka_pb2.Response(errcode=0)

            self.ensure_mode(jkzuc.MODE_AUTO)

            global MTN_ABORT
            MTN_ABORT = False

            c.auto(jkzuc.AUTO_PAUSE)
            grpc_logger().debug("[gRPC] program paused")
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ProgResume(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            if not s.traj_paused:
                return jaka_pb2.Response(errcode=0)
            if s.task_mode not in (jkzuc.MODE_AUTO, jkzuc.MODE_MDI):
                return jaka_pb2.Response(errcode=0)
            self.ensure_mode(jkzuc.MODE_AUTO, jkzuc.MODE_MDI)
            c.auto(jkzuc.AUTO_RESUME)
            grpc_logger().debug("[gRPC] program resumed")
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetJointLimit(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            minJointPosLimits = s.robot_model_data["minJointPosLimits"]
            maxJointPosLimits = s.robot_model_data["maxJointPosLimits"]

            joint_limits = request.jointlimits.joint_limit
            for joint in joint_limits:
                # grpc_logger().debug('{} {} {}'.format(joint.joint_id,joint.negative_limit,joint.positive_limit pengpeng))
                if not (
                    joint.positive_limit <= maxJointPosLimits[joint.joint_id]
                    and joint.negative_limit >= minJointPosLimits[joint.joint_id]
                    and joint.positive_limit > joint.negative_limit
                ):

                    errMsg = "joint {:d} min_limit is too small or max_limit is too large or min_limit not less than max_limit".format(
                        joint.joint_id
                    )
                    grpc_logger().debug("[gRPC] " + errMsg)
                    return jaka_pb2.Response(errcode=6, errormsg=errMsg)

                c.robot_set_joint_min_limit(request.robot_index, joint.joint_id, joint.negative_limit)
                c.robot_set_joint_max_limit(request.robot_index, joint.joint_id, joint.positive_limit)

            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetJointLimit(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            resp = jaka_pb2.Response(errcode=0)
            joint_limit_sets = resp.jointlimits
            config = configparser.ConfigParser()
            config.read(INI_FILE)
            section = config.sections()
            for joint_section in section:
                if joint_section.find("JOINT") >= 0:
                    joint = joint_limit_sets.joint_limit.add()
                    joint.joint_id = int(joint_section.split("_")[1])
                    joint.positive_limit = float(
                        config.get(joint_section, "JOINT_MAX_LIMIT")
                    )
                    joint.negative_limit = float(
                        config.get(joint_section, "JOINT_MIN_LIMIT")
                    )
            return resp
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTrajMaxVel(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.maxvel(request.flo)
            grpc_logger().debug(
                "[gRPC] max traj velocity set: {:.3f}".format(request.flo)
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    # @async_call
    # def _SetVelRate(self, velRate):
    #     c.rapidrate(velRate)

    def SetVelRate(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            # self._SetVelRate(request.flo)
            c.rapidrate(request.flo)
            grpc_logger().debug("[gRPC] rapid rate set: {:.3f}".format(request.flo))
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ExitProtectStat(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        try:
            c.exit_protect(request.intid)
            if request.intid == 0:
                grpc_logger().debug("[gRPC] exit collision protective mode")
            if request.intid == 1:
                grpc_logger().debug("[gRPC] exit safeguard mode")
            if request.intid == 2:
                grpc_logger().debug("[gRPC] SafeZone confirmation")
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetCurrentFile(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            currFile = os.path.basename(s.file)
            return jaka_pb2.Response(errcode=0, str=currFile)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetCurrentLine(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            return jaka_pb2.Response(errcode=0, intid=s.task_executing_line)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def KineInverse(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            robot_pose = request.robotpose
            robot_index = request.robot_index
            cart_pos = (
                robot_pose.cartpos.x,
                robot_pose.cartpos.y,
                robot_pose.cartpos.z,
                robot_pose.cartpos.a,
                robot_pose.cartpos.b,
                robot_pose.cartpos.c,
            )
            joint_pos = (
                robot_pose.jointpos.joint1,
                robot_pose.jointpos.joint2,
                robot_pose.jointpos.joint3,
                robot_pose.jointpos.joint4,
                robot_pose.jointpos.joint5,
                robot_pose.jointpos.joint6,
                robot_pose.jointpos.joint7,
            )
            resflag, jnt1, jnt2, jnt3, jnt4, jnt5, jnt6, jnt7 = c.kine_inverse(
                robot_index, cart_pos, joint_pos
            )

            jointsval = jaka_pb2.JointPos(
                joint1=jnt1,
                joint2=jnt2,
                joint3=jnt3,
                joint4=jnt4,
                joint5=jnt5,
                joint6=jnt6,
                joint7=jnt7,
            )
            if resflag:
                grpc_logger().debug("[gRPC] kinematics inverse failed")
                return jaka_pb2.Response(
                    errcode=7, errormsg="Failure to find kinematics inverse"
                )
            return jaka_pb2.Response(errcode=0, jointpos=jointsval)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def KineForward(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            joints = (
                request.jointpos.joint1,
                request.jointpos.joint2,
                request.jointpos.joint3,
                request.jointpos.joint4,
                request.jointpos.joint5,
                request.jointpos.joint6,
                request.jointpos.joint7,
            )
            resflag, px, py, pz, pa, pb, pc = c.kine_forward(request.robot_index, joints)

            cart_pos = jaka_pb2.CartPos(x=px, y=py, z=pz, a=pa, b=pb, c=pc)
            print(cart_pos,resflag)
            if resflag:
                grpc_logger().debug("[gRPC] kinematics forward failed")
                return jaka_pb2.Response(
                    errcode=8, errormsg="Failure to find kinematics forward"
                )
            return jaka_pb2.Response(errcode=0, cartpos=cart_pos)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def CheckFileMd5(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            file_info = request.checkfilemd5
            file_path = jkzuc.jkroot_dir + "/scripts/"
            md5_hash = hashlib.md5()
            try:
                fd = open(file_path + file_info.file_name, "rb")
            except Exception:
                grpc_logger().debug(
                    "[gRPC] No such file or directory: %s"
                    % (file_path + file_info.file_name)
                )
                return jaka_pb2.Response(
                    errcode=9, errormsg="No such file or directory"
                )
            block = fd.read(3)
            if b"\xef\xbb\xbf" in block:
                block = block.replace(b"\xef\xbb\xbf", b"")
            md5_hash.update(block)
            while True:
                block = fd.read(8096)
                if not block:
                    break
                md5_hash.update(block)
            fd.close()
            if md5_hash.hexdigest() != file_info.file_md5:
                grpc_logger().debug(
                    "MD5 mismatch. Expected: {}, Actual: {}".format(
                        file_info.file_md5, md5_hash.hexdigest()
                    )
                )
                return jaka_pb2.Response(errcode=10, errormsg="md5 is not match.")
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetRobotPose(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            jpos = s.joint_actual_position
            cpos = s.actual_position
            jpos_res = jaka_pb2.JointPos(
                joint1=jpos[0],
                joint2=jpos[1],
                joint3=jpos[2],
                joint4=jpos[3],
                joint5=jpos[4],
                joint6=jpos[5],
                joint7=jpos[6],
            )
            cpos_res = jaka_pb2.CartPos(
                x=cpos[0], y=cpos[1], z=cpos[2], a=cpos[3], b=cpos[4], c=cpos[5]
            )
            robot_pose = jaka_pb2.RobotPose()
            robot_pose.jointpos.CopyFrom(jpos_res)
            robot_pose.cartpos.CopyFrom(cpos_res)

            return jaka_pb2.Response(errcode=0, robotpose=robot_pose)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetPayload(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            com = [request.payload.xcom, request.payload.ycom, request.payload.zcom]
            c.set_payload(request.robot_index, request.payload.value, com)
            grpc_logger().debug(
                "[gRPC] set payload: {:.3f}, {:.3f}, {:.3f}, {:.3f}".format(
                    request.payload.value,
                    request.payload.xcom,
                    request.payload.ycom,
                    request.payload.zcom,
                )
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetReducedModeLevelValue(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            if s.percentage_mode_level != 0:
                return jaka_pb2.Response(
                    errcode=27, errormsg="failed to set reducing rate in reduced mode"
                )
            if (
                request.reducedmodelevelvalue.reducedmodelevel1
                < request.reducedmodelevelvalue.reducedmodelevel2
            ):
                return jaka_pb2.Response(
                    errcode=-1,
                    errormsg="Para Error! Level 2 must be lower than Level 1",
                )
            if (
                request.reducedmodelevelvalue.reducedmodelevel1 < 0
                or request.reducedmodelevelvalue.reducedmodelevel1 > 1
            ):
                return jaka_pb2.Response(
                    errcode=-1, errormsg="Para Error! Level must be within 0 and 1"
                )
            if (
                request.reducedmodelevelvalue.reducedmodelevel2 < 0
                or request.reducedmodelevelvalue.reducedmodelevel2 > 1
            ):
                return jaka_pb2.Response(
                    errcode=-1, errormsg="Para Error! Level must be within 0 and 1"
                )
            # 缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容grpc接口相关不做名称变更
            c.set_percentage_mode_level_value(
                request.reducedmodelevelvalue.reducedmodelevel1,
                request.reducedmodelevelvalue.reducedmodelevel2,
            )
            grpc_logger().debug(
                "[gRPC] set percentage mode level value: {:.3f}, {:.3f}".format(
                    request.reducedmodelevelvalue.reducedmodelevel1,
                    request.reducedmodelevelvalue.reducedmodelevel2,
                )
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetReducedModeLevelValue(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            reduceRateVal = jaka_pb2.ReducedModeLevelValue()
            update_jkzuc_status()
            # 缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容grpc接口相关不做名称变更
            percentageModeVal = s.percentage_mode_level_val
            reduceRateVal.reducedmodelevel1 = percentageModeVal[0]
            reduceRateVal.reducedmodelevel2 = percentageModeVal[1]
            return jaka_pb2.Response(errcode=0, reducedmodelevelvalue=reduceRateVal)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetupAnalogChannel(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.intid >= 0 and request.intid < 2:
                # 0 for analog channel
                c.setup_io_channel(0, request.intid, request.anaChannelSetting)
                c.wait_complete()
                grpc_logger().debug(
                    "[gRPC] setup analog channel: {:d}, {:d}".format(
                        request.intid, request.anaChannelSetting
                    )
                )
                time.sleep(0.2)
                update_jkzuc_status()
                res = jaka_pb2.Response()
                res.intid = request.intid
                res.anaChnSettings.append(s.scb_anachn_setting[request.intid])
                return res
            return jaka_pb2.Response(errcode=-1, errormsg="Invalid arguments!")
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetAnalogChannelSetting(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            res = jaka_pb2.Response()
            if s.cab_type == 2:
                scbAnaChnSettings = s.scb_anachn_setting
                for chnSetting in scbAnaChnSettings:
                    res.anaChnSettings.append(chnSetting)
            return res
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetPayload(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            pay_load = jaka_pb2.Payload()
            pay_load.value = s.payload[0]
            pay_load.xcom = s.payload[1][0]
            pay_load.ycom = s.payload[1][1]
            pay_load.zcom = s.payload[1][2]
            return jaka_pb2.Response(errcode=0, payload=pay_load)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetZucVersion(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))

        try:
            update_jkzuc_status()

            if s.powered_on == 1:
                servo_version, tmp = tool_function.getServoVerStr(s.servo_version)
                tio_version = tool_function.getTioVer(s.tio_version)[0]
                tio_ver = str(s.tio_version)
            elif s.powered_on == 0:
                servo_version = "0.0"
                tio_version = "TIO-0.0_0.0"
                tio_ver = "0"

            scb_version, tmp = tool_function.getScbVerStr(s)
            ctrl_version = jkzuc.controller_version + "_scb" + scb_version

            zuc_version = jaka_pb2.ZucVersion(
                controller_version = ctrl_version,
                servo_version = servo_version + "  " + tio_version,
                tio_version = tio_ver,
                scb_version = scb_version
            )
            return jaka_pb2.Response(errcode=0, zucversion=zuc_version)
        except MiddlewareException:  # as err:
            zuc_version = jaka_pb2.ZucVersion(
                controller_version=jkzuc.controller_version,
                servo_version="0",
                tio_version="0",
                scb_version="0"
            )
            return jaka_pb2.Response(errcode=0, zucversion=zuc_version)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def is_valid_systempackage(self, path):
        upgrade_package = os.listdir(os.path.join(path))[0]
        checkpath = os.path.join(path, upgrade_package, "Install", "Makefile")
        grpc_logger().debug("check system package path: {}".format(checkpath))
        return os.path.isfile(checkpath)

    def CheckIfx86_64(self):
        return platform.machine() == "x86_64"

    def UpgradeSystem(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            file_path = jkzuc.jkroot_dir + "/scripts/" + request.str
            real_path = os.path.realpath(file_path)
            scripts_path = os.path.realpath(jkzuc.jkroot_dir + "/scripts/")
            if (scripts_path in real_path) == False:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_PERMISSION_DENY, errormsg="permission denied"
                )
            if not os.path.exists(file_path):
                grpc_logger().debug("[gRPC] upgrade package doesnot exit")
                return jaka_pb2.Response(
                    errcode=9, errormsg="Upgrade package does not exist."
                )

            os.chdir(jkzuc.jkroot_dir + "/scripts/")
            os.system("rm -rf jkzuc;mkdir jkzuc")

            ret = 0
            if file_path.endswith(".tar.gz"):
                ret = os.system("tar -zxvf %s -C ./jkzuc" % request.str)
            elif file_path.endswith(".zip"):
                ret = os.system("unzip %s -d ./jkzuc" % request.str)
            if ret != 0:
                grpc_logger().debug("[gRPC] upgrade package is invalid")
                return jaka_pb2.Response(
                    errcode=21, errormsg="Upgrade package is invalid."
                )
            os.system("rm -rf %s" % request.str)
            if (
                self.is_valid_systempackage(os.path.join(scripts_path, "jkzuc"))
                == False
            ):
                os.system("rm -rf jkzuc")
                return jaka_pb2.Response(
                    errcode=21, errormsg="Upgrade package is invalid."
                )

            grpc_logger().debug("[gRPC] UpgradeSystem called")
            c.state(1)
            # try:
            #     downgrade_handle_ctx = downgrade_handle.ModuleCtx(grpc_logger)
            #     res = downgrade_handle.run(request.str, downgrade_handle_ctx)
            #     if res != 0:
            #         grpc_logger().debug("downgrade_handle failed")
            # except Exception:
            #     grpc_logger().debug(traceback.format_exc())

            threading.Timer(1, timer_reboot).start()
            # reboot_proc = multiprocessing.Process(target=timer_reboot)
            # reboot_proc.start()
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetGrpcErrcodeFromAddon(self, addon_errcode):
        errcode_map = {
            addon.addon_exception_code.RES_SUCC: jaka_pb2.RPC_OK,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONVERT_FAILED: jaka_pb2.RPC_ADDON_ERROR_CONVERT,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONVERT_JSON_READ_FAILED: jaka_pb2.RPC_ADDON_ERROR_JSON_READ,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONVERT_CHECK_FAILED: jaka_pb2.RPC_ADDON_ERROR_CHECK,
            addon.addon_exception_code.EXCEPTION_SERVICE_INSTALL_PACKAGE_NOT_EXIST: jaka_pb2.RPC_FILE_DIR_NOT_FOUND,
            addon.addon_exception_code.EXCEPTION_SERVICE_INSTALL_PACKAGE_MOVE_FAILED: jaka_pb2.RPC_INVALID_UPGRADE_PKCG,
            addon.addon_exception_code.EXCEPTION_SERVICE_INSTALL_PACKAGE_UNCOMPRESS_FAILED: jaka_pb2.RPC_INVALID_UPGRADE_PKCG,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONFIG_FILE_INVALID: jaka_pb2.RPC_ADDON_CONFIG_INVALID,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONFIG_FILE_ERR_CONVENTION: jaka_pb2.RPC_ADDON_CONFIG_UNKOWN_CONVENTION,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONFIG_FILE_MISS_REQUIRED: jaka_pb2.RPC_ADDON_CONFIG_MISSING_REQUIRED,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONFIG_FILE_ERR_LANGUAGE: jaka_pb2.RPC_ADDON_CONFIG_ERR_LANGUAGE,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONFIG_FILE_ERR_TYPE: jaka_pb2.RPC_ADDON_CONFIG_ERR_TYPE,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONFIG_FILE_ERR_SERVICEENABLE: jaka_pb2.RPC_ADDON_CONFIG_ERR_SERVICEENABLE,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONFIG_FILE_ERR_SERVICE: jaka_pb2.RPC_ADDON_CONFIG_ERR_SERVICE,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONFIG_FILE_ERR_NAME: jaka_pb2.RPC_ADDON_CONFIG_ERR_NAME,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONFIG_FILE_ERR_DESCRIPTION: jaka_pb2.RPC_ADDON_CONFIG_ERR_DESCRIPTION,
            addon.addon_exception_code.EXCEPTION_SERVICE_CONFIG_FILE_ERR_URL: jaka_pb2.RPC_ADDON_CONFIG_ERR_URL,
            addon.addon_exception_code.EXCEPTION_SERVICE_ALLOCATE_PORT_FAILED: jaka_pb2.RPC_ADDON_CONFIG_ERR_PORT,
            addon.addon_exception_code.EXCEPTION_SERVICE_INSTALL_PACKAGE_THREAD_IS_BUSY: jaka_pb2.RPC_ADDON_BUSY_INSTALLER,
            addon.addon_exception_code.EXCEPTION_SERVICE_INSTALL_PACKAGE_COMPRESS_FAILED: jaka_pb2.RPC_ADDON_PACK_FAILED,
            addon.addon_exception_code.EXCEPTION_SERVICE_INSTALL_PACKAGE_TIMEOUT: jaka_pb2.RPC_ADDON_TIMEOUT,
        }
        grpc_errcode = errcode_map.get(addon_errcode)
        return grpc_errcode if grpc_errcode is not None else jaka_pb2.RPC_EXCEPT

    def InstallAddon(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        grpc_logger().debug("grpc {}".format(request.str))
        try:
            if not self.CheckIfx86_64():
                # 32位系统暂不支持AddOn
                return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK)

            file_path = jkzuc.jkroot_dir + "/scripts/" + request.str
            res, addoninfo, msg = addon_controller_inst.install(file_path)
            errcode = self.GetGrpcErrcodeFromAddon(res)
            if errcode == jaka_pb2.RPC_OK:
                addoninfo = jaka_pb2.AddOnInfo()
                addoninfo.CopyFrom(addoninfo)
                return jaka_pb2.Response(errcode=errcode, addoninfo=addoninfo)

            if errcode == jaka_pb2.RPC_EXCEPT:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )

            return jaka_pb2.Response(errcode=errcode, errormsg=msg)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def InstallAddOnAsync(self, request, _context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        grpc_logger().debug("grpc {}".format(request.str))
        try:
            if not self.CheckIfx86_64():
                # 32位系统暂不支持AddOn
                return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK)

            file_path = jkzuc.jkroot_dir + "/scripts/" + request.str
            res, _, msg = addon_controller_inst.install_async(file_path)
            errcode = self.GetGrpcErrcodeFromAddon(res)
            if errcode == jaka_pb2.RPC_OK:
                return jaka_pb2.Response(errcode=0)

            if errcode == jaka_pb2.RPC_EXCEPT:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )

            return jaka_pb2.Response(errcode=errcode, errormsg=msg)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetAddOnList(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        if not self.CheckIfx86_64():
            # 32位系统暂不支持AddOn
            addonlist = jaka_pb2.AddOnList()
            return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK, addonlist=addonlist)

        def to_grpc_list(addon_info_list):
            grpc_addonlist = jaka_pb2.AddOnList()
            for addon_info in addon_info_list:
                # 返回结果中字典的key均为全小写，GRPC中仅有这两个为非全小写
                # 转换时需要单独处理
                keys = ["serviceEnabled", "languageType"]
                for k in keys:
                    v = addon_info.pop(k.lower(), None)
                    if v is not None:
                        addon_info[k] = v
            # grpc_logger().info("info is : {}".format(addon_info))
            json_format.Parse(
                json.dumps({"addOnInfoList": addon_info_list}),
                grpc_addonlist,
                ignore_unknown_fields=True,
            )
            return grpc_addonlist

        try:
            res, ret_data, msg = addon_controller_inst.get_addon_list()
            errcode = self.GetGrpcErrcodeFromAddon(res)
            if errcode == jaka_pb2.RPC_OK:
                addonlist = to_grpc_list(ret_data)
                return jaka_pb2.Response(errcode=0, addonlist=addonlist)

            if errcode == jaka_pb2.RPC_EXCEPT:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )

            return jaka_pb2.Response(errcode=errcode, errormsg=msg)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetAddOnServiceStatus(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        if not self.CheckIfx86_64():
            # 32位系统暂不支持AddOn
            serviceStatus = jaka_pb2.AddonServiceStatus()
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_OK, addonServiceStatus=serviceStatus
            )

        try:
            serviceStatus = jaka_pb2.AddonServiceStatus()
            res, status_dict, msg = addon_controller_inst.get_service_status()
            # grpc_logger().info('[GRPC -> AddOnMgr] res {} status_dict {}'.format(res, status_dict))
            errcode = self.GetGrpcErrcodeFromAddon(res)
            if errcode == jaka_pb2.RPC_EXCEPT:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
                )

            if errcode != jaka_pb2.RPC_OK:
                return jaka_pb2.Response(errcode=errcode, errormsg=msg)

            serviceStatus.infos.update(status_dict)
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_OK, addonServiceStatus=serviceStatus
            )
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    # todo logsomething
    def StartMachineAutoRunAddonService(self, request, context):  # deprecated
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            raise DeprecatedGrpcException()
        except Exception:
            grpc_logger().error(traceback.format_exc())
            grpc_logger().debug(
                "[AddOn]: autorun err:{} ".format(traceback.format_exc())
            )
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetAddonFilePermissions(self):
        pass

    def ExportAddOnFiles(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if not self.CheckIfx86_64():
                # 32位系统暂不支持AddOn
                return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK)

            filename = request.str
            res, _, msg = addon_controller_inst.pack_addon(filename)
            errcode = self.GetGrpcErrcodeFromAddon(res)
            if errcode == jaka_pb2.RPC_OK:
                return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK)

            if errcode == jaka_pb2.RPC_EXCEPT:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
                )

            return jaka_pb2.Response(errcode=errcode, errormsg=msg)
        except Exception:
            grpc_logger().debug(
                "[AddOn]: ExportAddOnFiles err:{} ".format(traceback.format_exc())
            )
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
            )

    def ExportAddOnFilesAsync(self, request, context):
        try:
            if not self.CheckIfx86_64():
                # 32位系统暂不支持AddOn
                return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK)

            filename = request.str
            res, _, msg = addon_controller_inst.pack_addon_async(filename)
            errcode = self.GetGrpcErrcodeFromAddon(res)
            if errcode == jaka_pb2.RPC_OK:
                return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK)

            if errcode == jaka_pb2.RPC_EXCEPT:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
                )

            return jaka_pb2.Response(errcode=errcode, errormsg=msg)
        except Exception:
            grpc_logger().debug(
                "[AddOn]: ExportAddOnFilesAsync err:{} ".format(traceback.format_exc())
            )
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
            )

    def GetCustomCommandContent(self):
        pass

    def AddOnServiceConfigCheck(self):
        pass

    def SetAddOnStaus(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        config = configparser.ConfigParser()
        try:
            if not self.CheckIfx86_64():
                # 32位系统暂不支持AddOn
                return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK)

            filename = jkzuc.jkroot_dir + "/configs/JAKA/AddOns"
            path = os.path.join(filename, request.setaddonstaus.filename)
            inifile_path = path + "/" + request.setaddonstaus.filename + "_config.ini"
            # self.CheckPathExists(inifile_path)
            service_name = request.setaddonstaus.filename + ".py"
            service_path = self.GetAddonServicePath(
                request.setaddonstaus.filename, service_name
            )
            config.read(inifile_path)
            if request.setaddonstaus.enabled_type > 0:
                return self.ExecuteAddonService(service_path, config, inifile_path)
            else:
                return self.StopAddonService(service_path, config, inifile_path)

        # except IOError:
        #     grpc_logger().debug(
        #         "[AddOn]:{} is not exist,please check AddOn config if exists".
        #         format(inifile_path))
        #     return jaka_pb2.Response(errcode=jaka_pb2.RPC_NO_SUCH_ADDON_CONFIG,\
        #                     errormsg='Exception:AddOn config is not exist')

        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ExecuteAddonService(self, service_path, config=None, inifile_path=""):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            res, _, msg = addon_controller_inst.start(service_path)
            # service_name = service_path.split('/')[-2]
            errcode = self.GetGrpcErrcodeFromAddon(res)
            if errcode == jaka_pb2.RPC_EXCEPT:
                return jaka_pb2.Response(
                    errcode=errcode, errormsg="Exception:grpc call failed"
                )

            if errcode == jaka_pb2.RPC_OK:
                grpc_logger().debug("[AddOn]:{} started ".format(service_path))
                return jaka_pb2.Response(errcode=errcode)

            return jaka_pb2.Response(errcode=errcode, errormsg=msg)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def StopAddonService(self, service_path, config, inifile_path):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            stop_ret, _, msg = addon_controller_inst.stop(service_path)
            errcode = self.GetGrpcErrcodeFromAddon(stop_ret)
            if errcode == jaka_pb2.RPC_EXCEPT:
                grpc_logger().debug("[AddOn]:{} stoped failed".format(service_path))
                return jaka_pb2.Response(
                    errcode=errcode, errormsg="Exception:grpc call failed"
                )

            if errcode == jaka_pb2.RPC_OK:
                grpc_logger().debug("[AddOn]:{} stoped".format(service_path))
                return jaka_pb2.Response(errcode=0)

            return jaka_pb2.Response(errcode=errcode, errormsg=msg)
        # except IOError:
        #     grpc_logger().debug(
        #         "[AddOn]:{} is not exist,please check AddOn config or service file"
        #         .format(service_path))
        #     return jaka_pb2.Response(errcode=jaka_pb2.RPC_NO_SUCH_ADDON_SERVICE,\
        #                     errormsg='Exception:AddOn service is not exist')

        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetAddonServicePath(self, dir_name, service_name):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        temp_path = jkzuc.jkroot_dir + "/configs/JAKA/AddOns"
        service_path = temp_path + "/" + dir_name + "/" + service_name
        return service_path

    def GetAddonServiceNameByDirName(self):
        pass

    def SetAddonServiceNameByDirName(self):
        pass

    def DeleteAddonServiceNameByDirName(self):
        pass

    def DeleteAddOn(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if not self.CheckIfx86_64():
                # 32位系统暂不支持AddOn
                return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK)

            filename = jkzuc.jkroot_dir + "/configs/JAKA/AddOns"
            path = os.path.join(filename, request.str)
            inifile_path = path + "/" + request.str + "_config.ini"
            res_code, _, msg = addon_controller_inst.delete_addon(inifile_path)
            errcode = self.GetGrpcErrcodeFromAddon(res_code)
            if errcode == jaka_pb2.RPC_OK:
                return jaka_pb2.Response(errcode=0)

            if errcode == jaka_pb2.RPC_EXCEPT:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )

            return jaka_pb2.Response(errcode=errcode, errormsg=msg)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ConvertAddOn(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if not self.CheckIfx86_64():
                # 32位系统暂不支持AddOn
                return jaka_pb2.ConvertAddOnRes(errcode=jaka_pb2.RPC_OK)

            res_code, _, msg = addon_controller_inst.convert_addon()
            errcode = self.GetGrpcErrcodeFromAddon(res_code)
            if errcode == jaka_pb2.RPC_OK:
                return jaka_pb2.ConvertAddOnRes(errcode=0)

            if errcode == jaka_pb2.RPC_EXCEPT:
                return jaka_pb2.ConvertAddOnRes(
                    errcode=1, errormsg="Exception:grpc call failed"
                )

            return jaka_pb2.ConvertAddOnRes(errcode=errcode, errormsg=msg)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.ConvertAddOnRes(
                errcode=1, errormsg="Exception:grpc call failed"
            )

    # 无实际意义
    def SetDhParams(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            dh_param_type = request.intid
            grpc_logger().debug("dh_param_type: {}".format(dh_param_type))
            dhoff = request.dhparam
            if dh_param_type == 1:
                err_alpha = (
                    dhoff.ahpha0,
                    dhoff.ahpha1,
                    dhoff.ahpha2,
                    dhoff.ahpha3,
                    dhoff.ahpha4,
                    dhoff.ahpha5,
                )
                err_a = (dhoff.a0, dhoff.a1, dhoff.a2, dhoff.a3, dhoff.a4, dhoff.a5)
                err_d = (dhoff.d1, dhoff.d2, dhoff.d3, dhoff.d4, dhoff.d5, dhoff.d6)
                err_homeoff = (
                    dhoff.joint_homeoff1,
                    dhoff.joint_homeoff2,
                    dhoff.joint_homeoff3,
                    dhoff.joint_homeoff4,
                    dhoff.joint_homeoff5,
                    dhoff.joint_homeoff6,
                )
                # c.calib_dhparam(err_alpha, err_a, err_d, err_homeoff)
            else:
                dh = [0] * 12
                update_jkzuc_status()
                dh = s.dh_param
                err_alpha = (
                    dhoff.ahpha0 - dh[0][0],
                    dhoff.ahpha1 - dh[0][1],
                    dhoff.ahpha2 - dh[0][2],
                    dhoff.ahpha3 - dh[0][3],
                    dhoff.ahpha4 - dh[0][4],
                    dhoff.ahpha5 - dh[0][5],
                )
                err_a = (
                    dhoff.a0 - dh[1][0],
                    dhoff.a1 - dh[1][1],
                    dhoff.a2 - dh[1][2],
                    dhoff.a3 - dh[1][3],
                    dhoff.a4 - dh[1][4],
                    dhoff.a5 - dh[1][5],
                )
                err_d = (
                    dhoff.d1 - dh[2][0],
                    dhoff.d2 - dh[2][1],
                    dhoff.d3 - dh[2][2],
                    dhoff.d4 - dh[2][3],
                    dhoff.d5 - dh[2][4],
                    dhoff.d6 - dh[2][5],
                )
                err_homeoff = (
                    dhoff.joint_homeoff1 - dh[3][0],
                    dhoff.joint_homeoff2 - dh[3][1],
                    dhoff.joint_homeoff3 - dh[3][2],
                    dhoff.joint_homeoff4 - dh[3][3],
                    dhoff.joint_homeoff5 - dh[3][4],
                    dhoff.joint_homeoff6 - dh[3][5],
                )
                # c.calib_dhparam(err_alpha, err_a, err_d, err_homeoff)
            grpc_logger().debug("[gRPC] calibrate DH Params")
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    @sim_mode_required(0)
    def GetDhParams(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            dh_paramoff = jaka_pb2.DhParam()
            dh = [0] * 12
            update_jkzuc_status()
            dh = s.dh_param
            dh_paramoff.alpha0 = dh[0][0]
            dh_paramoff.alpha1 = dh[0][1]
            dh_paramoff.alpha2 = dh[0][2]
            dh_paramoff.alpha3 = dh[0][3]
            dh_paramoff.alpha4 = dh[0][4]
            dh_paramoff.alpha5 = dh[0][5]
            dh_paramoff.alpha6 = dh[0][6]

            dh_paramoff.a0 = dh[1][0]
            dh_paramoff.a1 = dh[1][1]
            dh_paramoff.a2 = dh[1][2]
            dh_paramoff.a3 = dh[1][3]
            dh_paramoff.a4 = dh[1][4]
            dh_paramoff.a5 = dh[1][5]
            dh_paramoff.a6 = dh[1][6]

            dh_paramoff.d1 = dh[2][0]
            dh_paramoff.d2 = dh[2][1]
            dh_paramoff.d3 = dh[2][2]
            dh_paramoff.d4 = dh[2][3]
            dh_paramoff.d5 = dh[2][4]
            dh_paramoff.d6 = dh[2][5]
            dh_paramoff.d7 = dh[2][6]

            dh_paramoff.joint_homeoff1 = dh[3][0]
            dh_paramoff.joint_homeoff2 = dh[3][1]
            dh_paramoff.joint_homeoff3 = dh[3][2]
            dh_paramoff.joint_homeoff4 = dh[3][3]
            dh_paramoff.joint_homeoff5 = dh[3][4]
            dh_paramoff.joint_homeoff6 = dh[3][5]
            dh_paramoff.joint_homeoff7 = dh[3][6]
            return jaka_pb2.Response(errcode=0, dhparam=dh_paramoff)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetControllerBoxTime(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            times = jaka_pb2.ControllerBoxTime()
            cmd = "+%Y_%m_%d_%H_%M_%S"
            curtime = time.strftime("%Y_%m_%d_%H_%M_%S", time.localtime())
            time_str = curtime.split("_")
            times.year = int(time_str[0])
            times.month = int(time_str[1])
            times.day = int(time_str[2])
            times.hour = int(time_str[3])
            times.minute = int(time_str[4])
            times.second = int(time_str[5])
            return jaka_pb2.Response(errcode=0, controllerboxtime=times)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetControllerBoxTime(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        request_date_time = "{}-{}-{} {}:{}:{}".format(
            str(request.controllerboxtime.year),
            str(request.controllerboxtime.month),
            str(request.controllerboxtime.day),
            str(request.controllerboxtime.hour),
            str(request.controllerboxtime.minute),
            str(request.controllerboxtime.second),
        )
        try:
            global IS_SETTING_CONTROLLER_BOX_TIME
            IS_SETTING_CONTROLLER_BOX_TIME = 1
            tool_function_ctx = tool_function.ModuleCtx(grpc_logger)
            tool_function.runCmdAsRoot(
                'settime "{}"'.format(request_date_time), tool_function_ctx
            )
            global CURRENT_TIME
            CURRENT_TIME = time.time()
            IS_SETTING_CONTROLLER_BOX_TIME = 0
            grpc_logger().debug(
                "[gRPC] set system time with cmd: {}".format(request_date_time)
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetIOName(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            std_di = {}
            std_ai = {}
            std_do = {}
            std_ao = {}
            tio_di = {}
            tio_ai = {}
            tio_do = {}
            tio_ao = {}
            ext_di = {}
            ext_ai = {}
            ext_do = {}
            ext_ao = {}
            mb_slave_di = {}
            mb_slave_ai = {}
            mb_slave_do = {}
            mb_slave_ao = {}
            pn_dev_di = {}
            pn_dev_ai = {}
            pn_dev_do = {}
            pn_dev_ao = {}
            eip_adpt_di = {}
            eip_adpt_ai = {}
            eip_adpt_do = {}
            eip_adpt_ao = {}

            global SET_NAME_STDIO_DI
            global SET_NAME_STDIO_DO
            global SET_NAME_STDIO_AI
            global SET_NAME_STDIO_AO

            global SET_NAME_TIO_DI
            global SET_NAME_TIO_DO
            global SET_NAME_TIO_AI
            global SET_NAME_TIO_AO

            global SET_NAME_EXTIO_DI
            global SET_NAME_EXTIO_DO
            global SET_NAME_EXTIO_AI
            global SET_NAME_EXTIO_AO

            global SET_NAME_MB_DI
            global SET_NAME_MB_DO
            global SET_NAME_MB_AI
            global SET_NAME_MB_AO

            global SET_NAME_PFN_DI
            global SET_NAME_PFN_DO
            global SET_NAME_PFN_AI
            global SET_NAME_PFN_AO

            global SET_NAME_EIP_DI
            global SET_NAME_EIP_DO
            global SET_NAME_EIP_AI
            global SET_NAME_EIP_AO

            update_jkzuc_status()
            for io in request.ioname.data_input:
                if io.type == 0:
                    if io.adtype == 0:
                        std_di.update({io.id: io.name})
                        SET_NAME_STDIO_DI.update({io.id: io.name})
                    elif io.adtype == 1:
                        std_ai.update({io.id: io.name})
                        SET_NAME_STDIO_AI.update({io.id: io.name})
                elif io.type == 1:
                    if io.adtype == 0:
                        tio_di.update({io.id: io.name})
                        SET_NAME_TIO_DI.update({io.id: io.name})
                    elif io.adtype == 1:
                        tio_ai.update({io.id: io.name})
                        SET_NAME_TIO_AI.update({io.id: io.name})
                elif io.type == 2:  # extio
                    # SET_NAME_EXTIO = 1
                    if io.adtype == 0:  # digital input
                        ext_di.update({io.id: io.name})
                        SET_NAME_EXTIO_DI.update({io.id: io.name})
                    elif io.adtype == 1:
                        ext_ai.update({io.id: io.name})
                        SET_NAME_EXTIO_AI.update({io.id: io.name})
                elif io.type == 4:  # modbus slave io
                    if io.adtype == 0:  # digital input
                        mb_slave_di.update({io.id: io.name})
                        SET_NAME_MB_DI.update({io.id: io.name})
                    elif io.adtype == 1:
                        mb_slave_ai.update({io.id: io.name})
                        SET_NAME_MB_AI.update({io.id: io.name})
                elif io.type == 5:  # profinet io
                    if io.adtype == 0:  # digital input
                        pn_dev_di.update({io.id: io.name})
                        SET_NAME_PFN_DI.update({io.id: io.name})
                    elif io.adtype == 1:
                        pn_dev_ai.update({io.id: io.name})
                        SET_NAME_PFN_AI.update({io.id: io.name})
                elif io.type == 6:  # ethernet/ip io
                    if io.adtype == 0:  # digital input
                        eip_adpt_di.update({io.id: io.name})
                        SET_NAME_EIP_DI.update({io.id: io.name})
                    elif io.adtype == 1:
                        eip_adpt_ai.update({io.id: io.name})
                        SET_NAME_EIP_AI.update({io.id: io.name})

            for io in request.ioname.data_output:
                if io.type == 0:
                    if io.adtype == 0:
                        std_do.update({io.id: io.name})
                        SET_NAME_STDIO_DO.update({io.id: io.name})
                    elif io.adtype == 1:
                        std_ao.update({io.id: io.name})
                        SET_NAME_STDIO_AO.update({io.id: io.name})
                elif io.type == 1:
                    if io.adtype == 0:
                        tio_do.update({io.id: io.name})
                        SET_NAME_TIO_DO.update({io.id: io.name})
                    elif io.adtype == 1:
                        tio_ao.update({io.id: io.name})
                        SET_NAME_TIO_AO.update({io.id: io.name})
                elif io.type == 2:  # extending io module
                    if io.adtype == 0:  # digital output
                        ext_do.update({io.id: io.name})
                        SET_NAME_EXTIO_DO.update({io.id: io.name})
                    elif io.adtype == 1:
                        ext_ao.update({io.id: io.name})
                        SET_NAME_EXTIO_AO.update({io.id: io.name})
                elif io.type == 4:  # modbus slave
                    if io.adtype == 0:  # digital output
                        mb_slave_do.update({io.id: io.name})
                        SET_NAME_MB_DO.update({io.id: io.name})
                    elif io.adtype == 1:
                        mb_slave_ao.update({io.id: io.name})
                        SET_NAME_MB_AO.update({io.id: io.name})
                elif io.type == 5:  #
                    if io.adtype == 0:  # digital output
                        pn_dev_do.update({io.id: io.name})
                        SET_NAME_PFN_DO.update({io.id: io.name})
                    elif io.adtype == 1:
                        pn_dev_ao.update({io.id: io.name})
                        SET_NAME_PFN_AO.update({io.id: io.name})
                elif io.type == 6:  #
                    if io.adtype == 0:  # digital output
                        eip_adpt_do.update({io.id: io.name})
                        SET_NAME_EIP_DO.update({io.id: io.name})
                    elif io.adtype == 1:
                        eip_adpt_ao.update({io.id: io.name})
                        SET_NAME_EIP_AO.update({io.id: io.name})

            kw = {
                "STD_DI": std_di,
                "STD_DO": std_do,
                "STD_AI": std_ai,
                "STD_AO": std_ao,
                "TIO_DI": tio_di,
                "TIO_DO": tio_do,
                "TIO_AI": tio_ai,
                "TIO_AO": tio_ao,
                "EXT_DI": ext_di,
                "EXT_DO": ext_do,
                "EXT_AI": ext_ai,
                "EXT_AO": ext_ao,
                "MB_SLAVE_DI": mb_slave_di,
                "MB_SLAVE_DO": mb_slave_do,
                "MB_SLAVE_AI": mb_slave_ai,
                "MB_SLAVE_AO": mb_slave_ao,
                "PN_DEV_DI": pn_dev_di,
                "PN_DEV_DO": pn_dev_do,
                "PN_DEV_AI": pn_dev_ai,
                "PN_DEV_AO": pn_dev_ao,
                "EIP_ADPT_DI": eip_adpt_di,
                "EIP_ADPT_DO": eip_adpt_do,
                "EIP_ADPT_AI": eip_adpt_ai,
                "EIP_ADPT_AO": eip_adpt_ao,
            }
            # 将joint limit 写入配置文件
            filename = jkzuc.jkroot_dir + "/configs/JAKA/aliasname.ini"
            self.save_config(filename, kw)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetIOName(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        update_jkzuc_status()
        try:
            io_name = jaka_pb2.IOName()
            filename = jkzuc.jkroot_dir + "/configs/JAKA/aliasname.ini"
            config = configparser.ConfigParser()
            config.read(filename)
            # input_list = ['STD_DI', 'STD_AI', 'TIO_DI', 'TIO_AI', 'EXT_DI', 'EXT_AI']
            input_map = {
                "STD_DI": {"type": jaka_pb2.IOTYPE_GPIO, "adtype": 0},
                "STD_AI": {"type": jaka_pb2.IOTYPE_GPIO, "adtype": 1},
                "TIO_DI": {"type": jaka_pb2.IOTYPE_TIO, "adtype": 0},
                "TIO_AI": {"type": jaka_pb2.IOTYPE_TIO, "adtype": 1},
                "EXT_DI": {"type": jaka_pb2.IOTYPE_MBIO, "adtype": 0},
                "EXT_AI": {"type": jaka_pb2.IOTYPE_MBIO, "adtype": 1},
                "MB_SLAVE_DI": {"type": jaka_pb2.IOTYPE_MBSIO, "adtype": 0},
                "MB_SLAVE_AI": {"type": jaka_pb2.IOTYPE_MBSIO, "adtype": 1},
                "PN_DEV_DI": {"type": jaka_pb2.IOTYPE_PNDEVIO, "adtype": 0},
                "PN_DEV_AI": {"type": jaka_pb2.IOTYPE_PNDEVIO, "adtype": 1},
                "EIP_ADPT_DI": {"type": jaka_pb2.IOTYPE_EIPADPTIO, "adtype": 0},
                "EIP_ADPT_AI": {"type": jaka_pb2.IOTYPE_EIPADPTIO, "adtype": 1},
            }
            for type_input, info_input in input_map.items():
                if not config.has_section(type_input):
                    continue
                for key, value in config.items(type_input):
                    io = io_name.data_input.add()
                    io.id = str(key)
                    io.name = str(value)
                    io.type = info_input["type"]
                    io.adtype = info_input["adtype"]

            output_map = {
                "STD_DO": {"type": jaka_pb2.IOTYPE_GPIO, "adtype": 0},
                "STD_AO": {"type": jaka_pb2.IOTYPE_GPIO, "adtype": 1},
                "TIO_DO": {"type": jaka_pb2.IOTYPE_TIO, "adtype": 0},
                "TIO_AO": {"type": jaka_pb2.IOTYPE_TIO, "adtype": 1},
                "EXT_DO": {"type": jaka_pb2.IOTYPE_MBIO, "adtype": 0},
                "EXT_AO": {"type": jaka_pb2.IOTYPE_MBIO, "adtype": 1},
                "MB_SLAVE_DO": {"type": jaka_pb2.IOTYPE_MBSIO, "adtype": 0},
                "MB_SLAVE_AO": {"type": jaka_pb2.IOTYPE_MBSIO, "adtype": 1},
                "PN_DEV_DO": {"type": jaka_pb2.IOTYPE_PNDEVIO, "adtype": 0},
                "PN_DEV_AO": {"type": jaka_pb2.IOTYPE_PNDEVIO, "adtype": 1},
                "EIP_ADPT_DO": {"type": jaka_pb2.IOTYPE_EIPADPTIO, "adtype": 0},
                "EIP_ADPT_AO": {"type": jaka_pb2.IOTYPE_EIPADPTIO, "adtype": 1},
            }
            for type_output, info_output in output_map.items():
                if not config.has_section(type_output):
                    continue
                for key, value in config.items(type_output):
                    io = io_name.data_output.add()
                    io.id = str(key)
                    io.name = str(value)
                    io.type = info_output["type"]
                    io.adtype = info_output["adtype"]
            return jaka_pb2.Response(errcode=0, ioname=io_name)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetAutoWork(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.autoworkmode.mode == 1 and request.autoworkmode.filename == "":
                grpc_logger().debug("[gRPC] Autoloader configuration failed. ")
                return jaka_pb2.Response(errcode=jaka_pb2.RPC_EMPTY_STRING)
            c.set_auto_work(
                request.autoworkmode.mode,
                jkzuc.jkroot_dir + "/scripts/" + request.autoworkmode.filename,
            )
            grpc_logger().debug(
                "[gRPC] set auto work: {} {}".format(
                    request.autoworkmode.mode, request.autoworkmode.filename
                )
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetAutoWork(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            work_mode = jaka_pb2.AutoWorkMode()
            work_mode.mode = s.auto_work[0]
            work_mode.filename = s.auto_work[1]
            return jaka_pb2.Response(errcode=0, autoworkmode=work_mode)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def CalculateTCP(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            jntPoints = []
            if request.calctcpreq.methodtype == 0:
                for jntPos in request.calctcpreq.jointpos:
                    jntPoint = (
                        jntPos.joint1,
                        jntPos.joint2,
                        jntPos.joint3,
                        jntPos.joint4,
                        jntPos.joint5,
                        jntPos.joint6,
                        jntPos.joint7,
                    )
                    jntPoints.append(jntPoint)
                resflag, tx, ty, tz, ta, tb, tc, tErrMax, tErrMin, tErrMean = (
                    c.calc_tcp(
                        request.calctcpreq.id,
                        request.calctcpreq.saveType,
                        request.calctcpreq.statusFlag,
                        jntPoints[0],
                        jntPoints[1],
                        jntPoints[2],
                        jntPoints[3],
                    )
                )
            elif request.calctcpreq.methodtype == 1:
                for jntPos in request.calctcpreq.jointpos:
                    jntPoint = (
                        jntPos.joint1,
                        jntPos.joint2,
                        jntPos.joint3,
                        jntPos.joint4,
                        jntPos.joint5,
                        jntPos.joint6,
                        jntPos.joint7,
                    )
                    jntPoints.append(jntPoint)
                resflag, tx, ty, tz, ta, tb, tc, tErrMax, tErrMin, tErrMean = (
                    c.calc_tcp_six(
                        request.calctcpreq.id,
                        request.calctcpreq.saveType,
                        request.calctcpreq.statusFlag,
                        jntPoints[0],
                        jntPoints[1],
                        jntPoints[2],
                        jntPoints[3],
                        jntPoints[4],
                        jntPoints[5],
                    )
                )
            cart_pos = jaka_pb2.CartPos(
                x=tx,
                y=ty,
                z=tz,
                a=ta,
                b=tb,
                c=tc,
                errMax=tErrMax,
                errMin=tErrMin,
                errMean=tErrMean,
            )
            if resflag:
                grpc_logger().debug("[gRPC] failed to calc tcp")
                return jaka_pb2.Response(
                    errcode=11, errormsg="Failure to calculate TCP"
                )
            grpc_logger().debug("[gRPC] succeed to calc tcp")
            return jaka_pb2.Response(errcode=0, cartpos=cart_pos)
        except Exception:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def CalculateUser(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            jntPoints = []
            for jntPos in request.calcUserReq.jointpos:
                jntPoint = (
                    jntPos.joint1,
                    jntPos.joint2,
                    jntPos.joint3,
                    jntPos.joint4,
                    jntPos.joint5,
                    jntPos.joint6,
                    jntPos.joint7,
                )
                jntPoints.append(jntPoint)
            resflag, tx, ty, tz, ta, tb, tc = c.calc_user(
                request.calcUserReq.id,
                request.calcUserReq.saveType,
                request.calcUserReq.statusFlag,
                jntPoints[0],
                jntPoints[1],
                jntPoints[2],
            )
            cart_pos = jaka_pb2.CartPos(x=tx, y=ty, z=tz, a=ta, b=tb, c=tc)
            if resflag:
                grpc_logger().debug("[gRPC] failed to calc user frame")
                return jaka_pb2.Response(
                    errcode=11, errormsg="Failure to calculate user"
                )
            grpc_logger().debug("[gRPC] succeed to calc user frame")
            return jaka_pb2.Response(errcode=0, cartpos=cart_pos)
        except Exception:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def DeleteFile(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        update_jkzuc_status()
        # 程序运行时不允许删除文件
        if s.interp_state != jkzuc.INTERP_IDLE:
            grpc_logger().warn("Program is running, can not delete script file ")
            return jaka_pb2.Response(
                errcode=0x002A,
                errormsg="The program is running and file deletion is not allowed",
            )  # 1.7.1版本新增的grpc errcode 可以按‘0xE0000 + errcode’  记录到code.csv 文件中，不用修改proto文件即可通过APP飘窗报错 （不可与proto文件中已有的errcode冲突）
        try:
            file_path = jkzuc.jkroot_dir + "/scripts/" + request.str
            real_path = os.path.abspath(file_path)
            scripts_path = jkzuc.jkroot_dir + "/"
            if (scripts_path in real_path) == False:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_PERMISSION_DENY, errormsg="permission denied"
                )
            if os.path.exists(file_path):
                os.system('rm -rf "%s"' % file_path)
                grpc_logger().debug(
                    "[gRPC] DeleteFile: file {} deleted".format(file_path)
                )
            else:
                grpc_logger().debug(
                    "[gRPC] DeleteFile: file {} does not exist".format(file_path)
                )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def RemoteCopy(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))

        global remoteCopyState
        scripts_path = jkzuc.jkroot_dir + "/scripts/"
        file_path = scripts_path + request.filecopy.src_path
        real_path = os.path.abspath(file_path)
        if (scripts_path in real_path) == False:
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_PERMISSION_DENY, errormsg="permission denied"
            )
        try:
            if os.path.exists(file_path):
                # xfer = ftptransfer.Xfer()
                # xfer.setFtpParams(request.filecopy.dst_path, 'jaka', 'jaka12345')
                prog_name = ""
                if os.path.isdir(file_path):
                    folders = file_path.split("/")
                    prog_name = folders[len(folders) - 1]
                    if prog_name.strip(" ") == "":
                        prog_name = folders[len(folders) - 2]
                # dirOverride, True: 已存在文件夹需要被覆盖, False: 检查同名文件夹, 已存在文件夹不需要被覆盖
                # 因为jog过程中,mtnabort调用本身可能执行时间会很长，所以采用子线程的方式运行，以避免app报断线
                remotCopyProcess = threading.Thread(
                    target=remoteCopyProcess,
                    args=(
                        request.filecopy.dst_path,
                        request.filecopy.src_path,
                        prog_name,
                        False,
                    ),
                )
                remotCopyProcess.daemon = True
                remotCopyProcess.start()
                remotCopyProcess.join(0.5)
                # if xfer.upload(file_path, './program/' + prog_name, False) == False:
                #     grpc_logger().debug('[gRPC] RemoteCopy: the directory is already existed!')
                #     return jaka_pb2.Response(errcode=1018, errormsg='the directory is already existed!')
                # grpc_logger().debug('[gRPC] RemoteCopy: complete from {} to {}'.format(file_path, './program/' + prog_name))
                if remoteCopyState == -1:
                    return jaka_pb2.Response(
                        errcode=0x0F0135, errormsg="Exception:grpc call failed"
                    )
            else:
                grpc_logger().debug(
                    "[gRPC] RemoteCopy: file {} does not exist".format(file_path)
                )
                c.set_grpc_errcode(
                    jkzuc.OPERATOR_ERROR, 0x0F0135, "read network ini failed,", 0xFF, 0
                )
                return jaka_pb2.Response(
                    errcode=0x0F0135, errormsg="Exception:grpc call failed"
                )
            return jaka_pb2.Response(errcode=0)
        except:
            err_str = traceback.format_exc()
            grpc_logger().error(err_str)
            pattern = r"\[Errno ([0-9]*)\]"
            res = re.search(pattern, err_str)
            errno_code = None
            if res:
                errno_code = int(res.group(1))
            if errno_code == 101:
                # network is unreachable
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_NETWORK_UNREACHABLE,
                    errormsg="Network is unreachable",
                )
            if errno_code == 111:
                # connection refused
                return jaka_pb2.Response(
                    errcode=jaka_pb2.CONNECTION_REFUSED, errormsg="Connection refused"
                )

            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def CopyFile(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            scripts_path = jkzuc.jkroot_dir + "/scripts/"
            src_path = scripts_path + request.filecopy.src_path
            dst_path = scripts_path + request.filecopy.dst_path
            real_src = os.path.realpath(src_path)
            real_dst = os.path.realpath(dst_path)
            scripts_path = jkzuc.jkroot_dir + "/scripts/"
            if ((real_src == real_dst) or (scripts_path in real_src) == False) or (
                (scripts_path in real_dst) == False
            ):
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_PERMISSION_DENY, errormsg="permission denied"
                )
            if not os.path.exists(src_path):
                grpc_logger().debug(
                    "[gRPC] LocalCopy: file {} does not exist".format(src_path)
                )
                return jaka_pb2.Response(
                    errcode=9, errormsg="No such file or directory"
                )

            self.sync_path(src_path)
            os.system('cp -r "%s" "%s"' % (src_path, dst_path))
            self.sync_path(dst_path)
            time.sleep(1)
            grpc_logger().debug(
                "[gRPC] LocalCopy: file copied from {} to {}".format(src_path, dst_path)
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetCameraList(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            camera_list = jaka_pb2.CameraList()
            dir_path = jkzuc.jkroot_dir + "/configs/vision/"
            if not os.path.exists(dir_path):
                return jaka_pb2.Response(
                    errcode=9, errormsg="No such file or directory"
                )
            for file in os.listdir(dir_path):
                camera_list.filename.append(file)
            return jaka_pb2.Response(errcode=0, cameralist=camera_list)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetDIFunc(self, request, context):  #
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if (
                request.diset.id < 0
                or request.diset.action > FUNCDI_NUM
                or request.diset.action < 0
            ):
                return jaka_pb2.Response(errcode=-1, errormsg="Invalid arguments!")

            update_jkzuc_status()
            currFuncDIMap = list(s.funcdi)
            grpc_logger().debug("request.diset.id: {}".format(request.diset.id))
            grpc_logger().debug("request.diset.action: {}".format(request.diset.action))
            # grpc_logger().debug("currFuncDIMap: {}, {}".format(currFuncDIMapItem[0], currFuncDIMapItem[1]))
            # grpc_logger().debug("request DI: {}, {}".format(request.diset.type, currFuncDIMapItem[1]))
            if request.diset.action != 0:  # valid action
                if request.diset.type == 0 and s.cab_type == 2:
                    if (
                        request.diset.id < 16
                        and s.safety_funcdi[request.diset.id % 8] > 1
                    ):
                        return jaka_pb2.Response(
                            errcode=26,
                            errormsg="IO channel occupied by safety function",
                        )
                currFuncDIMapItem = currFuncDIMap[request.diset.action - 1]
                if (
                    currFuncDIMapItem[0] != -1 and currFuncDIMapItem[1] != -1
                ):  # this function has been occupied
                    if (
                        currFuncDIMapItem[0] != request.diset.type
                        or currFuncDIMapItem[1] != request.diset.id
                    ):
                        return jaka_pb2.Response(
                            errcode=jaka_pb2.RPC_DI_BINDS_EXCEED,
                            errormsg="two many inputs bind to the same function",
                        )
                    else:
                        return jaka_pb2.Response(errcode=0)
            global cabMaxDIO
            global modMaxDIO
            for idx in range(0, FUNCDI_NUM):
                currFuncDIMap[idx] = list(currFuncDIMap[idx])
                # 若设置相同IO则先清除
                if (
                    currFuncDIMap[idx][0] == request.diset.type
                    and currFuncDIMap[idx][1] == request.diset.id
                ):
                    currFuncDIMap[idx] = [-1, -1]
                    break
                elif (
                    currFuncDIMap[idx][0] == 0
                    and currFuncDIMap[idx][1] == request.diset.id + cabMaxDIO
                    and request.diset.type == 4
                ):
                    currFuncDIMap[idx] = [-1, -1]
                    break
                elif (
                    currFuncDIMap[idx][0] == 0
                    and currFuncDIMap[idx][1]
                    == request.diset.id + cabMaxDIO + modMaxDIO
                    and request.diset.type == 5
                ):
                    currFuncDIMap[idx] = [-1, -1]
                    break

            if request.diset.action != 0:  # valid action
                currFuncDIMap[request.diset.action - 1] = [
                    request.diset.type,
                    request.diset.id,
                ]
                if request.diset.type == 4:
                    currFuncDIMap[request.diset.action - 1] = [
                        0,
                        request.diset.id + cabMaxDIO,
                    ]
                elif request.diset.type == 5:
                    currFuncDIMap[request.diset.action - 1] = [
                        0,
                        request.diset.id + cabMaxDIO + modMaxDIO,
                    ]

            c.set_funcdi(currFuncDIMap)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetDIFunc(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            currFuncDIMap = s.funcdi
            dimap = jaka_pb2.GetDIFunc()
            for idx in range(0, FUNCDI_NUM):
                if currFuncDIMap[idx][0] >= 0 and currFuncDIMap[idx][1] >= 0:
                    dimapItem = dimap.disets.add()
                    dimapItem.type = currFuncDIMap[idx][0]
                    dimapItem.id = currFuncDIMap[idx][1]
                    dimapItem.action = idx + 1
            return jaka_pb2.Response(errcode=0, getdifunc=dimap)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetDOFunc(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if (
                request.doset.type < 0
                or request.doset.id < 0
                or request.doset.action > FUNCDO_NUM
            ):
                return jaka_pb2.Response(errcode=12, errormsg="Invalid arguments!")

            update_jkzuc_status()
            currFuncDOMap = list(s.funcdo)
            global cabMaxDIO
            global modMaxDIO
            for actIdx in range(0, FUNCDO_NUM):
                currFuncDOMap[actIdx] = list(currFuncDOMap[actIdx])
                # remove the duplicated IO
                tempMap = []
                for item in currFuncDOMap[actIdx]:
                    if item in tempMap and item != [-1, -1]:
                        item = [-1, -1]
                    tempMap.append(list(item))

                currFuncDOMap[actIdx] = [[-1, -1], [-1, -1], [-1, -1], [-1, -1]]
                currFuncDOMap[actIdx][0 : len(tempMap)] = tempMap
                for optIdx in range(len(currFuncDOMap[actIdx])):
                    currFuncDOMap[actIdx][optIdx] = list(currFuncDOMap[actIdx][optIdx])
                    if (
                        currFuncDOMap[actIdx][optIdx][0] == request.doset.type
                        and currFuncDOMap[actIdx][optIdx][1] == request.doset.id
                    ):
                        currFuncDOMap[actIdx][optIdx] = [-1, -1]
                        break
                    elif (
                        currFuncDOMap[actIdx][optIdx][0] == 0
                        and currFuncDOMap[actIdx][optIdx][1]
                        == request.doset.id + cabMaxDIO
                        and request.doset.type == 4
                    ):
                        currFuncDOMap[actIdx][optIdx] = [-1, -1]
                        break
                    elif (
                        currFuncDOMap[actIdx][optIdx][0] == 0
                        and currFuncDOMap[actIdx][optIdx][1]
                        == request.doset.id + cabMaxDIO + modMaxDIO
                        and request.doset.type == 5
                    ):
                        currFuncDOMap[actIdx][optIdx] = [-1, -1]
                        break

            if request.doset.action > 0:
                if request.doset.type == 0 and s.cab_type == 2:
                    if (
                        request.doset.id < 16
                        and s.safety_funcdo[request.doset.id % 8] > 1
                    ):
                        return jaka_pb2.Response(
                            errcode=26,
                            errormsg="IO channel occupied by safety function",
                        )

                set_ret = 0
                for optIdx in range(len(currFuncDOMap[request.doset.action - 1])):
                    grpc_logger().debug(
                        "{} {}".format(
                            optIdx, currFuncDOMap[request.doset.action - 1][optIdx]
                        )
                    )
                    if (
                        currFuncDOMap[request.doset.action - 1][optIdx][0] == -1
                        or currFuncDOMap[request.doset.action - 1][optIdx][1] == -1
                    ):
                        currFuncDOMap[request.doset.action - 1][optIdx][
                            0
                        ] = request.doset.type
                        currFuncDOMap[request.doset.action - 1][optIdx][
                            1
                        ] = request.doset.id
                        if request.doset.type == 4:
                            currFuncDOMap[request.doset.action - 1][optIdx][0] = 0
                            currFuncDOMap[request.doset.action - 1][optIdx][1] = (
                                request.doset.id + cabMaxDIO
                            )
                        elif request.doset.type == 5:
                            currFuncDOMap[request.doset.action - 1][optIdx][0] = 0
                            currFuncDOMap[request.doset.action - 1][optIdx][1] = (
                                request.doset.id + cabMaxDIO + modMaxDIO
                            )
                        set_ret = 1
                        break

                print("modified-currFuncDOMap: ", currFuncDOMap)
                if set_ret == 0:
                    return jaka_pb2.Response(
                        errcode=jaka_pb2.RPC_DO_BINDS_EXCEED,
                        errormsg="two many outputs bind to the same status",
                    )
            c.set_funcdo(currFuncDOMap)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetDOFunc(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            currFuncIOMap = s.funcdo
            domap = jaka_pb2.GetDOFunc()
            for actIdx in range(0, FUNCDO_NUM):  #
                for optIdx in range(len(currFuncIOMap[actIdx])):
                    optInst = currFuncIOMap[actIdx][optIdx]
                    if optInst[0] >= 0 and optInst[1] >= 0:
                        domapItem = domap.dosets.add()
                        domapItem.type = optInst[0]
                        domapItem.id = optInst[1]
                        domapItem.action = actIdx + 1
            return jaka_pb2.Response(errcode=0, getdofunc=domap)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def AddUserDefinedVariable(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            id_list = []
            alias_list = []
            variable_new = jaka_pb2.Variable()

            filename = jkzuc.jkroot_dir + "/configs/JAKA/jaka_user.var"
            with open(filename, "r+") as fd:
                for line in fd.readlines():
                    line = line.rstrip()
                    item_list = line.split("\t")

                    id_list.append(int(item_list[0]))
                    alias_list.append(str(item_list[2]))

            variable_new.value = request.variable.value

            for alias in alias_list:
                if alias == request.variable.alias:
                    return jaka_pb2.Response(
                        errcode=14, errormsg="The variable alias already exists"
                    )
            variable_new.alias = request.variable.alias
            if has_special_chars(variable_new.alias) or len(variable_new.alias) > 128:
                grpc_logger().debug(
                    "Paramater 'alias_new' check [ FAILED ] {}".format(
                        variable_new.alias
                    )
                )
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )

            if 5500 not in id_list:
                variable_new.id = 5500
            else:
                id_list.sort()
                for id in id_list:
                    id_index = id_list.index(id)
                    if id_index + 1 < len(id_list):
                        if (id + 1) != id_list[id_index + 1]:
                            variable_new.id = id + 1
                            break
                    else:
                        variable_new.id = id + 1
                    # 最多100 个系统变量
                    if variable_new.id >= 5600:
                        return jaka_pb2.Response(
                            errcode=15, errormsg="Parameter value out of range "
                        )

            c.set_user_defined_variable(variable_new.id, variable_new.value)
            # 将 variable 写入配置文件
            line = (
                str(variable_new.id)
                + "\t"
                + str(variable_new.value)
                + "\t"
                + str(variable_new.alias)
                + "\n"
            )
            with open(filename, "a+") as fd:
                fd.write(line)
            grpc_logger().debug(
                "[gRPC] add user variable: {} {}".format(
                    variable_new.id, variable_new.value
                )
            )
            return jaka_pb2.Response(errcode=0, variable=variable_new)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def DeleteUserDefinedVariable(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            line_list = []
            filename = jkzuc.jkroot_dir + "/configs/JAKA/jaka_user.var"
            with open(filename, "r+") as fd:
                line_list = fd.readlines()
                for id in request.deleteudv.id:
                    if id < 5500 or id > 5600:
                        return jaka_pb2.Response(
                            errcode=15, errormsg="Parameter value out of range "
                        )
                    grpc_logger().debug("[gRPC] delete user variable: {}".format(id))
                    for line in line_list:
                        line_id = int(line.split("\t")[0])
                        if id == line_id:
                            line_list.remove(line)
                            break
            for id in request.deleteudv.id:
                c.set_user_defined_variable(id, 0.0)
            with open(filename, "w") as fd:
                fd.writelines(line_list)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ModifyUserDefinedVariable(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            line_list = []
            id_list = []
            alias_list = []
            id_new = request.variable.id
            alias_new = request.variable.alias
            value_new = request.variable.value

            if has_special_chars(alias_new) or len(alias_new) > 128:
                grpc_logger().debug(
                    "Paramater 'alias_new' check [ FAILED ] {}".format(alias_new)
                )
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )
            filename = jkzuc.jkroot_dir + "/configs/JAKA/jaka_user.var"
            with open(filename, "r+") as fd:
                line_list = fd.readlines()
                for line in line_list:
                    line = line.rstrip()
                    item_list = line.split("\t")

                    id_list.append(int(item_list[0]))
                    alias_list.append(str(item_list[2]))

            for id in id_list:
                if id_new == id:
                    id_index = id_list.index(id)
                    alias_list.pop(id_index)
                    for alias in alias_list:
                        if alias_new == alias:
                            return jaka_pb2.Response(
                                errcode=14, errormsg="The variable alias already exists"
                            )

                    line_list[id_index] = (
                        str(id_new)
                        + "\t"
                        + str(value_new)
                        + "\t"
                        + str(alias_new)
                        + "\n"
                    )
                    break
            c.set_user_defined_variable(id_new, value_new)
            with open(filename, "w") as fd:
                fd.writelines(line_list)

            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def QueryUserDefinedVariable(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        try:
            user_variable = jaka_pb2.UserVariable()
            filename = jkzuc.jkroot_dir + "/configs/JAKA/jaka_user.var"
            with open(filename, "r+") as fd:
                for line in fd.readlines():
                    line = line.rstrip()
                    item_list = line.split("\t")

                    variable = user_variable.variable.add()
                    variable.id = int(item_list[0])
                    try:
                        tempValue = float(item_list[1])
                        variable.value = tempValue
                    except Exception:
                        variable.value = float("inf")
                    variable.alias = str(item_list[2])

            return jaka_pb2.Response(errcode=0, uservariable=user_variable)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def Shutdown(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            grpc_logger().debug("[gRPC] system shutdown")
            # c.state(1)
            c.shut_down()
            # threading.Timer(2, self.timer_shutdown).start()
            # shut_proc = multiprocessing.Process(target=self.timer_shutdown)
            # shut_proc.start()
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetToolOffset(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            self.ensure_mode(jkzuc.MODE_MANUAL)
            c.tool_offset(
                request.cartpos.x,
                request.cartpos.y,
                request.cartpos.z,
                request.cartpos.a,
                request.cartpos.b,
                request.cartpos.c,
            )
            grpc_logger().debug("[gRPC] set tool offset: {}".format(request.cartpos))
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetToolOffset(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            offset = s.tool_offset
            tool_offset = jaka_pb2.CartPos(
                x=offset[0],
                y=offset[1],
                z=offset[2],
                a=offset[3],
                b=offset[4],
                c=offset[5],
            )
            return jaka_pb2.Response(errcode=0, cartpos=tool_offset)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def TeleopEnable(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.teleop_enable(request.intid)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    @sim_mode_required(0)
    def SetRobotID(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        if request.intid == 0:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:Intid invalid")
        c.set_cab_id(request.intid)
        return jaka_pb2.Response(errcode=0)

    def SetRobotName(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if len(request.str) > 128:
                return jaka_pb2.Response(
                    errcode=17, errormsg="Strings should not exceed 128 characters"
                )
            c.set_robot_name(request.str)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetUserList(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            file_name = jkzuc.jkroot_dir + "/configs/JAKA/jakar7_user.ini"
            user_info = {"username": "", "password": ""}
            kw = {
                "administrator": user_info,
                "operator": user_info,
                "visitor": user_info,
            }
            user_dict = self.read_config(file_name, kw)

            user_list = jaka_pb2.UserList()
            for key in user_dict:
                user_level = user_list.userinfo.add()
                user_level.level = key
                user_level.name = user_dict[key]["username"]
                user_level.password = user_dict[key]["password"]
                grpc_logger().debug(user_level.level)
                grpc_logger().debug(user_level.name)
                grpc_logger().debug(user_level.password)
            return jaka_pb2.Response(errcode=0, userlist=user_list)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ChangeUserInfo(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            file_name = jkzuc.jkroot_dir + "/configs/JAKA/jakar7_user.ini"
            userinfo = request.userinfo
            if userinfo.level not in ['visitor', 'operator', 'administrator']:
                return jaka_pb2.Response(errcode=19, errormsg="Invalid user level !")

            user_info = {'username': '', 'password': ''}
            kw = {'administrator': user_info, 'operator': user_info, 'visitor': user_info}
            user_dict = self.read_config(file_name, kw)

            for key in user_dict:
                if key != userinfo.level and userinfo.name == user_dict[key]['username']:
                    return jaka_pb2.Response(errcode=20, errormsg="The username has been used !")

            md5_password = userinfo.password
            if self.username == user_dict['administrator']['username'] or self.username == userinfo.name:
                new_user = {userinfo.level: {'username': userinfo.name, 'password': md5_password}}
                self.save_config(file_name, new_user)
                if self.username == userinfo.name:
                    self.LOGSTATUS = False
                    Robot.client_uq_IPA = ''
                    self.username = ''
                    self.userlevel = ''
                grpc_logger().info('[gRPC] user {} info changed'.format(userinfo.name))
            else:
                return jaka_pb2.Response(errcode=18, errormsg='Permission denied.')
            return jaka_pb2.Response(errcode=0)
        except Exception:
            print(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg='Exception:grpc call failed')

    def SetClsnSensitivity(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.intid < 0 or request.intid > 5:
                return jaka_pb2.Response(errcode=12, errormsg="Invalid arguments")

            sensitivityVal = request.intid
            c.set_clsn_sensi(sensitivityVal)
            grpc_logger().debug(
                "[gRPC] set collision sensitivity: {}".format(sensitivityVal)
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetClsnSensitivity(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            sensitivityLevel = s.clsn_sensitivity
            return jaka_pb2.Response(errcode=0, intid=sensitivityLevel)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetInstallationAngle(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            self.ensure_mode(jkzuc.MODE_MANUAL)
            grpc_logger().debug(
                "quaternion: s {}, x {}, y {}, z {}".format(
                    request.quaternion.s,
                    request.quaternion.x,
                    request.quaternion.y,
                    request.quaternion.z,
                )
            )
            installquat = [
                request.quaternion.s,
                request.quaternion.x,
                request.quaternion.y,
                request.quaternion.z,
            ]
            appang = [
                request.quaternion.angleX,
                request.quaternion.angleY,
                request.quaternion.angleZ,
            ]
            c.robot_set_base_offset(request.robot_index, 0, 0, 0, appang[0], appang[1], appang[2])
            grpc_logger().debug(
                "[gRPC] set installation angle: {} {}".format(installquat, appang)
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetInstallationAngle(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            quat, appang = s.installation_angle
            install_angle = jaka_pb2.Quaternion()
            install_angle.s = quat[0]
            install_angle.x = quat[1]
            install_angle.y = quat[2]
            install_angle.z = quat[3]
            install_angle.angleX = appang[0]
            install_angle.angleY = appang[1]
            install_angle.angleZ = appang[2]

            return jaka_pb2.Response(errcode=0, quaternion=install_angle)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetMutiToolOffset(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            self.ensure_mode(jkzuc.MODE_MANUAL)
            for tool in request.mutitooloffset.toolInfo:
                if tool.id == 0:
                    return jaka_pb2.Response(
                        errcode=jaka_pb2.RPC_INVALID_ARGS,
                        errormsg="Illegal modification of base coordinate system",
                    )
                tooloff = [
                    tool.tooloffset.x,
                    tool.tooloffset.y,
                    tool.tooloffset.z,
                    tool.tooloffset.a,
                    tool.tooloffset.b,
                    tool.tooloffset.c,
                ]
                c.set_muti_tool_offsets(tooloff, tool.method, tool.id, tool.name)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetSafeZone(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.mode(1)
            matrix = [[0 for x in range(6)] for y in range(4)]
            for i in range(0, 4):
                for j in range(0, 6):
                    matrix[i][j] = 0.0

            request.zone.safe = 1  # by default safe
            c.set_safe_zone(
                request.zone.openEnable,
                request.zone.autoEnable,
                request.zone.safeZoneMode,
                request.zone.safe,
                request.zone.elbow_pos_limit_enable,
                request.zone.wrist_pos_limit_enable,
                matrix,
                0,
                0.0,
                -1,
                "null",
            )
            for plane in request.zone.plane:
                i = 0
                j = 0
                for point in plane.points:
                    plane_point = [point.x, point.y, point.z, point.a, point.b, point.c]
                    matrix[j] = plane_point
                    j = j + 1
                i = i + 1
                safe_point = [
                    plane.safePoint.x,
                    plane.safePoint.y,
                    plane.safePoint.z,
                    plane.safePoint.a,
                    plane.safePoint.b,
                    plane.safePoint.c,
                ]
                matrix[3] = safe_point
                try:
                    print("safe zone set start !")
                    print(
                        "param {}".format(
                            (
                                request.zone.openEnable,
                                request.zone.autoEnable,
                                request.zone.safeZoneMode,
                                request.zone.safe,
                                request.zone.elbow_pos_limit_enable,
                                request.zone.wrist_pos_limit_enable,
                                matrix,
                                plane.enable,
                                plane.safeDist,
                                plane.id,
                                plane.planeName,
                            )
                        )
                    )
                    c.set_safe_zone(
                        request.zone.openEnable,
                        request.zone.autoEnable,
                        request.zone.safeZoneMode,
                        request.zone.safe,
                        request.zone.elbow_pos_limit_enable,
                        request.zone.wrist_pos_limit_enable,
                        matrix,
                        plane.enable,
                        plane.safeDist,
                        plane.id,
                        plane.planeName,
                    )
                    print("safe zone set end !")
                except Exception as e:
                    traceback.print_exc()
                # c.set_safe_zone(request.zone.openEnable,request.zone.autoEnable,0,request.zone.safe,matrix,plane.enable,plane.safeDist,plane.id,plane.planeName)

            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetSafeZoneEnable(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        # temporarily not used
        pass

    def GetSafeZone(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            safeZone = jaka_pb2.SafeZone()
            zone = s.safe_zone
            safeZone.openEnable = zone.get("openEnable")
            safeZone.autoEnable = zone.get("autoEnable")
            safeZone.safeZoneMode = zone.get("safeZoneMode")
            safeZone.elbow_pos_limit_enable = zone.get("elbowPosLimitEnable")
            safeZone.wrist_pos_limit_enable = zone.get("wristPosLimitEnable")
            id = 0
            for plane in zone.get("planes"):
                planeInfo = safeZone.plane.add()
                planeInfo.id = id
                id = id + 1
                planeInfo.planeName = plane.get("planeName")
                planeInfo.enable = plane.get("planeEnable")
                planeInfo.safeDist = plane.get("safeDist")
                planeInfo.safePoint.x = plane.get("safePoint").get("x")
                planeInfo.safePoint.y = plane.get("safePoint").get("y")
                planeInfo.safePoint.z = plane.get("safePoint").get("z")
                planeInfo.safePoint.a = plane.get("safePoint").get("a")
                planeInfo.safePoint.b = plane.get("safePoint").get("b")
                planeInfo.safePoint.c = plane.get("safePoint").get("c")
                for point in plane.get("planePoints"):
                    points = planeInfo.points.add()
                    points.x = point.get("x")
                    points.y = point.get("y")
                    points.z = point.get("z")
                    points.a = point.get("a")
                    points.b = point.get("b")
                    points.c = point.get("c")
            return jaka_pb2.Response(errcode=0, zone=safeZone)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def CalculateUserPointToWorld(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            cartpos = request.cartpos
            cart_pos_for_user = (
                cartpos.x,
                cartpos.y,
                cartpos.z,
                cartpos.a,
                cartpos.b,
                cartpos.c,
            )
            cart_pos_for_world = c.user_cartesian_point_convert_to_world(
                cart_pos_for_user
            )
            result_cart_pos = jaka_pb2.CartPos(
                x=cart_pos_for_world[0],
                y=cart_pos_for_world[1],
                z=cart_pos_for_world[2],
                a=cart_pos_for_world[3],
                b=cart_pos_for_world[4],
                c=cart_pos_for_world[5],
            )
            return jaka_pb2.Response(errcode=0, cartpos=result_cart_pos)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def CalculateWorldPointToUser(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            cartpos = request.cartpos
            cart_pos_for_world = (
                cartpos.x,
                cartpos.y,
                cartpos.z,
                cartpos.a,
                cartpos.b,
                cartpos.c,
            )
            cart_pos_for_user = c.world_cartesian_point_convert_to_user(
                cart_pos_for_world
            )
            result_cart_pos = jaka_pb2.CartPos(
                x=cart_pos_for_user[0],
                y=cart_pos_for_user[1],
                z=cart_pos_for_user[2],
                a=cart_pos_for_user[3],
                b=cart_pos_for_user[4],
                c=cart_pos_for_user[5],
            )
            return jaka_pb2.Response(errcode=0, cartpos=result_cart_pos)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetPlannerJerk(self, request, context):
        try:
            c.mode(1)
            c.set_planner_jerk(request.setJerk.joint_jerk, request.setJerk.carte_jerk)

            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetPlannerJerk(self, request, context):
        try:
            s.poll()
            setJerk = jaka_pb2.PlannerJerk()
            config = configparser.ConfigParser()
            config.read(INI_FILE)
            setJerk.joint_jerk = float(config.get("TRAJ", "TRAJ_MAX_JOINT_JERK"))
            setJerk.carte_jerk = float(config.get("TRAJ", "TRAJ_MAX_CARTE_JERK"))
            return jaka_pb2.Response(errcode=0, setJerk=setJerk)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def CalculateSafeAtt(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            cartpos_s = []
            for cartpos in request.calcsafeatt.cartpos:
                cartpos = (
                    cartpos.x,
                    cartpos.y,
                    cartpos.z,
                    cartpos.a,
                    cartpos.b,
                    cartpos.c,
                )
                cartpos_s.append(cartpos)
            print(
                "cartpos: {}, tilt: {}, pan: {}, tipChoice: {}".format(
                    cartpos_s,
                    request.calcsafeatt.tilt,
                    request.calcsafeatt.pan,
                    request.calcsafeatt.tipChoice,
                )
            )
            resflag, tr, tp, ty, devia = c.calc_safeatt(
                cartpos_s[0],
                cartpos_s[1],
                cartpos_s[2],
                request.calcsafeatt.tilt,
                request.calcsafeatt.pan,
                request.calcsafeatt.tipChoice,
            )
            # grpc_logger().debug("resflag:%i,tr:%d,tp:%d,ty:%d,devia:%d"%(resflag, tr, tp, ty, devia))
            safeatt = jaka_pb2.Safeatt(
                Conical_R=tr, Conical_P=tp, Conical_Y=ty, deviation=devia
            )
            # ('safeatt : Conical_R = ',safeatt.Conical_R,'Conical_Y = ',safeatt.Conical_Y,'Conical_Z = ',safeatt.Conical_Z,'deviation = ',safeatt.deviation)
            if resflag:
                grpc_logger().debug("[gRPC] failed to calc SafeAtt")
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_FAILED_TO_CALCULATE_SAFE_ATT,
                    errormsg="Failure to calculate SafeAtt",
                )
            grpc_logger().debug("[gRPC] succeed to calc SafeAtt")
            return jaka_pb2.Response(errcode=0, safeatt=safeatt)
        except Exception:
            grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetSafeAttitude(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.mode(1)
            conical_rpy = [
                request.attitudeLimit.Conical_RPY.x,
                request.attitudeLimit.Conical_RPY.y,
                request.attitudeLimit.Conical_RPY.z,
            ]
            c.set_safe_attitude(
                conical_rpy,
                request.attitudeLimit.tilt,
                request.attitudeLimit.pan,
                request.attitudeLimit.deviation,
                request.attitudeLimit.deviationwarn,
                request.attitudeLimit.openEnable,
                request.attitudeLimit.autoEnable,
                request.attitudeLimit.tipChoice,
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetSafeAttitude(self, request, context):
        try:
            update_jkzuc_status()
            attitudeLimit = jaka_pb2.SafeAttitude()
            config = configparser.ConfigParser()
            config.read(INI_FILE)
            conical_RPY_data = str(config.get("ROBOTSAFETYSETTING", "CONICAL_CENRPY"))
            trans_table = str.maketrans("", "", "{}")
            conical_RPY_data = conical_RPY_data.translate(trans_table)
            conical_RPY = conical_RPY_data.split(",")
            fconical_RPY = list(map(eval, conical_RPY))
            attitudeLimit.Conical_RPY.x = fconical_RPY[0]
            attitudeLimit.Conical_RPY.y = fconical_RPY[1]
            attitudeLimit.Conical_RPY.z = fconical_RPY[2]
            attitudeLimit.tilt = float(
                config.get("ROBOTSAFETYSETTING", "TOOL_PROPERTIES_TILT")
            )
            attitudeLimit.pan = float(
                config.get("ROBOTSAFETYSETTING", "TOOL_PROPERTIES_PAN")
            )
            attitudeLimit.deviation = float(
                config.get("ROBOTSAFETYSETTING", "DEVIATION")
            )
            attitudeLimit.deviationwarn = float(
                config.get("ROBOTSAFETYSETTING", "DEVIATION_WARNING")
            )
            attitudeLimit.openEnable = int(
                config.get("ROBOTSAFETYSETTING", "LIMIT_ATTITUDE_OPEN")
            )
            attitudeLimit.autoEnable = int(
                config.get("ROBOTSAFETYSETTING", "LIMIT_ATTITUDE_AUTO")
            )
            attitudeLimit.tipChoice = int(
                config.get("ROBOTSAFETYSETTING", "CONICAL_TIP_CHOICE")
            )
            return jaka_pb2.Response(errcode=0, attitudeLimit=attitudeLimit)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetMutiUserOffset(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            self.ensure_mode(jkzuc.MODE_MANUAL)
            for user in request.mutiuseroffset.userOffsetInfo:
                if user.id == 0:
                    return jaka_pb2.Response(
                        errcode=jaka_pb2.RPC_INVALID_ARGS,
                        errormsg="Illegal modification of base coordinate system",
                    )
                useroff = [
                    user.useroffset.x,
                    user.useroffset.y,
                    user.useroffset.z,
                    user.useroffset.a,
                    user.useroffset.b,
                    user.useroffset.c,
                ]
                c.set_muti_user_offsets(useroff, user.method, user.id, user.name)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetMutiToolOffset(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            #TODO 返回方法及名称
            update_jkzuc_status()
            tool_offset_sets = jaka_pb2.MutiToolOffset()
            tool_offset_sets.currentId = s.current_tool_id
            for tcpId in range(0, 16):
                tool = tool_offset_sets.toolInfo.add()
                tool.method = int(0)
                tool.id = tcpId
                tool.tooloffset.x = s.tool_offsets[tcpId][0]
                tool.tooloffset.y = s.tool_offsets[tcpId][1]
                tool.tooloffset.z = s.tool_offsets[tcpId][2]
                tool.tooloffset.a = s.tool_offsets[tcpId][3]
                tool.tooloffset.b = s.tool_offsets[tcpId][4]
                tool.tooloffset.c = s.tool_offsets[tcpId][5]
                tool.name = "TCP_" + str(tcpId)

            return jaka_pb2.Response(errcode=0, mutitooloffset=tool_offset_sets)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    # @FIXME liwang 7 轴适配
    def GetTCPCalibration(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            tcpId = request.id
            tcpRes = jaka_pb2.CalcTcpRes()
            config = configparser.ConfigParser()
            coordinateFile = jkzuc.jkroot_dir + "/configs/JAKA/calibrationsettings.ini"
            config.read(coordinateFile)
            tcp_section = "TCPCALIBRATION_" + str(tcpId)
            method = int(config.get(tcp_section, "METHOD"))
            statusFlag_tcp = int(config.get(tcp_section, "STATUSFLAG"))
            if method == 0:
                N = 4
            elif method == 1:
                N = 6
            tcp_err_str = str(config.get(tcp_section, "TCPERROR"))
            tcp_err_transtable = str.maketrans("", "", "{}")
            tcp_err_str = tcp_err_str.translate(tcp_err_transtable)
            tcp_err_data = tcp_err_str.split(",")
            tcp_err_map = list(map(eval, tcp_err_data))
            tcpRes.methodtype = method
            tcpRes.statusFlag = statusFlag_tcp

            for i in range(0, 3):
                tcpRes.tcpErr.append(tcp_err_map[i])

            for idx in range(0, N):
                carte_key = "CARTE_" + str(idx)
                carte_str = str(config.get(tcp_section, carte_key))
                trans_table = str.maketrans("", "", "{}")
                carte_str = carte_str.translate(trans_table)
                carte_data = carte_str.split(",")
                cartePos = list(map(eval, carte_data))
                tcpRes.cartPos.add(
                    x=cartePos[0],
                    y=cartePos[1],
                    z=cartePos[2],
                    a=cartePos[3],
                    b=cartePos[4],
                    c=cartePos[5],
                )

                joint_key = "JOINT_" + str(idx)
                joint_str = str(config.get(tcp_section, joint_key))
                joint_str = joint_str.translate(trans_table)
                joint_data = joint_str.split(",")
                jointPos = list(map(eval, joint_data))
                tcpRes.jointpos.add(
                    joint1=jointPos[0],
                    joint2=jointPos[1],
                    joint3=jointPos[2],
                    joint4=jointPos[3],
                    joint5=jointPos[4],
                    joint6=jointPos[5],
                )

            tcpRes.errcode = jaka_pb2.RPC_OK
            return tcpRes
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.CalcTcpRes(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
            )

    def GetMutiUserOffset(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            user_offset_sets = jaka_pb2.MutiUserOffset()
            user_offset_sets.currentId = s.current_user_id
            config = configparser.ConfigParser()
            config.read(INI_FILE)

            for userFrameId in range(0, 16):
                user_frame_entry = "USER_FRAME_" + str(userFrameId)
                frame = user_offset_sets.userOffsetInfo.add()
                  #TODO 返回方法及名称
                frame.method = int(0)
                frame.id = userFrameId
                frame.useroffset.x = s.user_offsets[userFrameId][0]
                frame.useroffset.y = s.user_offsets[userFrameId][1]
                frame.useroffset.z = s.user_offsets[userFrameId][2]
                frame.useroffset.a = s.user_offsets[userFrameId][3]
                frame.useroffset.b = s.user_offsets[userFrameId][4]
                frame.useroffset.c = s.user_offsets[userFrameId][5]
                frame.name = user_frame_entry
            return jaka_pb2.Response(errcode=0, mutiuseroffset=user_offset_sets)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    # @FIXME liwang 7 轴适配
    def GetUserCalibration(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            usrId = request.id
            usrRes = jaka_pb2.CalcUserRes()
            config = configparser.ConfigParser()
            coordinateFile = jkzuc.jkroot_dir + "/configs/JAKA/calibrationsettings.ini"
            config.read(coordinateFile)
            usr_section = "USERCALIBRATION_" + str(usrId)
            statusFlag_user = int(config.get(usr_section, "STATUSFLAG"))
            usrRes.statusFlag = statusFlag_user
            for idx in range(0, 3):
                carte_key = "CARTE_" + str(idx)
                carte_str = str(config.get(usr_section, carte_key))
                trans_table = str.maketrans("", "", "{}")
                carte_str = carte_str.translate(trans_table)
                carte_data = carte_str.split(",")
                cartePos = list(map(eval, carte_data))
                usrRes.cartPos.add(
                    x=cartePos[0],
                    y=cartePos[1],
                    z=cartePos[2],
                    a=cartePos[3],
                    b=cartePos[4],
                    c=cartePos[5],
                )

                joint_key = "JOINT_" + str(idx)
                joint_str = str(config.get(usr_section, joint_key))
                joint_str = joint_str.translate(trans_table)
                joint_data = joint_str.split(",")
                jointPos = list(map(eval, joint_data))
                usrRes.jointpos.add(
                    joint1=jointPos[0],
                    joint2=jointPos[1],
                    joint3=jointPos[2],
                    joint4=jointPos[3],
                    joint5=jointPos[4],
                    joint6=jointPos[5],
                )

            usrRes.errcode = jaka_pb2.RPC_OK
            return usrRes
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.CalcUserRes(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
            )

    def getIsSteppingMode(self, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            getSteppingModeRes = jaka_pb2.SteppingModeRes()
            getSteppingModeRes.isSteppingMode = s.is_stepping_mode

            getSteppingModeRes.errcode = jaka_pb2.RPC_OK
            return getSteppingModeRes
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.SteppingModeRes(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
            )

    def GetVelRate(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            return jaka_pb2.Response(errcode=0, flo=s.traj_rapidrate)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetCurrentToolId(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            self.ensure_mode(jkzuc.MODE_MANUAL)
            c.set_tool_id(request.robot_index, request.intid)
            grpc_logger().debug("[gRPC] set robot{} current tool id: {}".format(request.robot_index, request.intid))
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetLocalDir(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            local_dir = jaka_pb2.GetLocalDir()
            scripts_path = jkzuc.jkroot_dir + "/scripts/"
            valid_path = jkzuc.jkroot_dir + "/"
            root_path = os.path.join(scripts_path, request.str)
            real_path = os.path.abspath(root_path)
            if (valid_path in real_path) == False:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_PERMISSION_DENY, errormsg="permission denied"
                )
            if not os.path.exists(root_path):
                return jaka_pb2.Response(
                    errcode=9, errormsg="No such file or direction"
                )
            file_list = os.listdir(root_path)
            for file_name in file_list:
                file = local_dir.file.add()
                file.filename = file_name
                file_path = os.path.join(root_path, file_name)
                if os.path.isfile(file_path):
                    file.type = 1
                    file.size = int(os.path.getsize(file_path) / 1000)
                elif os.path.isdir(file_path):
                    if self.is_program_folder(file_path):
                        file.type = 2
                    else:
                        file.type = 3
                    file.size = int(self.get_folder_size(file_path) / 1000)
                else:
                    file.type = 0
                    file.size = 0
                ctime = time.localtime(os.path.getmtime(file_path))
                file.datetime = time.strftime("%Y-%m-%d %H:%M:%S", ctime)
            return jaka_pb2.Response(errcode=0, getlocaldir=local_dir)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetExtIOBriefInfo(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            extioInfo = jaka_pb2.ExtIOBriefInfo()
            extioInfo.modnum = s.extio_num
            for index in range(extioInfo.modnum):
                modName = s.extio_setup[index][1]
                extioInfo.modname.append(modName)
            return jaka_pb2.Response(errcode=0, extioinfo=extioInfo)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetExtIOModConfig(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            extIONum = s.extio_num
            if request.intid < 0 or request.intid >= extIONum:
                return jaka_pb2.Response(errcode=12, errormsg="Invalid arguments")

            # expected format of pyIOConfig
            # 'pinmap': [[di, do, ai, ao], ...]
            # 'setup': [(0, 'rtu', (9600, 8, 1), [(di_startAddr, di_num), (do1, do2), (ai1, ai2), (ao1, ao2)]), ...]
            extio_setup = s.extio_setup[request.intid]
            ioModSetup = jaka_pb2.ExtIOModSetup()
            ioModSetup.type = extio_setup[0]
            ioModSetup.modName = extio_setup[1]
            if ioModSetup.type == 0:  # MOD-RTU
                ioModSetup.rtuComm.baudrate = extio_setup[2][0]
                ioModSetup.rtuComm.bits = extio_setup[2][1]
                ioModSetup.rtuComm.datalength = extio_setup[2][1]
                ioModSetup.rtuComm.slaveId = extio_setup[2][2]
                ioModSetup.rtuComm.stopbit = extio_setup[2][4]
                ioModSetup.rtuComm.parity = extio_setup[2][3]
                if (
                    request.extiosetup.rtuComm.slaveId > 247
                    or request.extiosetup.rtuComm.slaveId < 0
                ):
                    grpc_logger().debug(
                        "slaveId %d out of range [%d,%d]"
                        % (request.extiosetup.rtuComm.slaveId, 0, 247)
                    )
                    return jaka_pb2.Response(
                        errcode=15, errormsg="Exception: parameter slaveId out of range"
                    )
            else:
                ioModSetup.tcpComm.ipaddr = extio_setup[2][0]
                ioModSetup.tcpComm.port = extio_setup[2][1]
                ioModSetup.tcpComm.slaveId = extio_setup[2][2]

            extio_pin_setup = extio_setup[3]
            ioModSetup.diSetup.startaddr = extio_pin_setup[0][0]
            ioModSetup.diSetup.pinnum = extio_pin_setup[0][1]
            ioModSetup.doSetup.startaddr = extio_pin_setup[1][0]
            ioModSetup.doSetup.pinnum = extio_pin_setup[1][1]
            ioModSetup.aiSetup.startaddr = extio_pin_setup[2][0]
            ioModSetup.aiSetup.pinnum = extio_pin_setup[2][1]
            ioModSetup.aoSetup.startaddr = extio_pin_setup[3][0]
            ioModSetup.aoSetup.pinnum = extio_pin_setup[3][1]

            ioModPinMap = jaka_pb2.ExtIOModPinMap()
            extio_pinmap = s.extio_pinmap[request.intid]
            ioModPinMap.diStartIdx = extio_pinmap[0]
            ioModPinMap.doStartIdx = extio_pinmap[1]
            ioModPinMap.aiStartIdx = extio_pinmap[2]
            ioModPinMap.aoStartIdx = extio_pinmap[3]

            ioModConfig = jaka_pb2.ExtIOModConfig()
            ioModConfig.extIOModSetup.CopyFrom(ioModSetup)
            ioModConfig.extIOModPinMap.CopyFrom(ioModPinMap)
            return jaka_pb2.Response(errcode=0, extioconfig=ioModConfig)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetExtIOMode(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_extio_mode(request.intid)
            grpc_logger().debug("[gRPC] set extio mode: {}".format(request.intid))
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def AddExtIOMod(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            # check  extio config constant

            in_constranit = self.check_extio_constraint(
                request.intid,
                request.extiosetup.diSetup.pinnum,
                request.extiosetup.doSetup.pinnum,
                request.extiosetup.aiSetup.pinnum,
                request.extiosetup.aoSetup.pinnum,
                True,
            )
            grpc_logger().debug("in_constranit: {}".format(in_constranit))
            if not in_constranit:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_EXTIO_CONSTRANIT,
                    errormsg="extio modoule number or extio io number exceed the constranit",
                )

            # get current IO module setups
            update_jkzuc_status()
            extIOSetups = []
            oldExtIOConfigs = [s.extio_pinmap, s.extio_setup]
            for extioSetup in oldExtIOConfigs[1]:
                if len(extioSetup[1]) != 0:  # 临时处理bug-9392 空扩展IO模块问题
                    extIOSetups.append(extioSetup)

            pyNewIOModSetup = []
            pyNewIOModSetup.append(request.extiosetup.type)
            pyNewIOModSetup.append(request.extiosetup.modName)
            if request.extiosetup.type == 0:
                pyRtuComm = [
                    request.extiosetup.rtuComm.baudrate,
                    request.extiosetup.rtuComm.datalength,
                    request.extiosetup.rtuComm.slaveId,
                    request.extiosetup.rtuComm.parity,
                    request.extiosetup.rtuComm.stopbit,
                ]
                pyNewIOModSetup.append(tuple(pyRtuComm))
                if (
                    request.extiosetup.rtuComm.slaveId > 247
                    or request.extiosetup.rtuComm.slaveId < 0
                ):
                    grpc_logger().debug(
                        "slaveId %d out of range [%d,%d]"
                        % (request.extiosetup.rtuComm.slaveId, 0, 247)
                    )
                    return jaka_pb2.Response(
                        errcode=15, errormsg="Exception: parameter slaveId out of range"
                    )

            else:
                pyTcpComm = [
                    request.extiosetup.tcpComm.ipaddr,
                    request.extiosetup.tcpComm.port,
                    request.extiosetup.tcpComm.slaveId,
                ]
                pyNewIOModSetup.append(tuple(pyTcpComm))

            pyPinSetup = []
            pyPinSetup.append(
                (
                    request.extiosetup.diSetup.startaddr,
                    request.extiosetup.diSetup.pinnum,
                )
            )
            pyPinSetup.append(
                (
                    request.extiosetup.doSetup.startaddr,
                    request.extiosetup.doSetup.pinnum,
                )
            )
            pyPinSetup.append(
                (
                    request.extiosetup.aiSetup.startaddr,
                    request.extiosetup.aiSetup.pinnum,
                )
            )
            pyPinSetup.append(
                (
                    request.extiosetup.aoSetup.startaddr,
                    request.extiosetup.aoSetup.pinnum,
                )
            )
            pyNewIOModSetup.append(pyPinSetup)

            pyNewIOModSetup.append(request.extiosetup.ExtIOModInfo)
            # pyNewIOModSetup.append("abc")
            extIOSetups.append(tuple(pyNewIOModSetup))
            grpc_logger().debug("extIOSetups: {}".format(extIOSetups))
            c.setup_extio(len(extIOSetups), extIOSetups)

            time.sleep(0.05)
            update_jkzuc_status()
            newExtIOConfigs = [s.extio_pinmap, s.extio_setup]
            self.updateExtIOName(oldExtIOConfigs, newExtIOConfigs)
            return jaka_pb2.Response(errcode=0)

        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ModifyExtIOMod(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            # check  extio config constant
            in_constranit = self.check_extio_constraint(
                request.intid,
                request.extiosetup.diSetup.pinnum,
                request.extiosetup.doSetup.pinnum,
                request.extiosetup.aiSetup.pinnum,
                request.extiosetup.aoSetup.pinnum,
                False,
            )

            if not in_constranit:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_EXTIO_CONSTRANIT,
                    errormsg="extio modoule number or extio io number exceed the constranit",
                )
            # get current IO module setups
            update_jkzuc_status()
            currExtIOSetups = []
            oldExtIOConfigs = [s.extio_pinmap, s.extio_setup]
            for extioSetup in oldExtIOConfigs[1]:
                currExtIOSetups.append(extioSetup)

            # ensure the module exists
            if request.intid < 0 or request.intid >= len(currExtIOSetups):
                return jaka_pb2.Response(errcode=12, errormsg="Invalid arguments")

            pyNewIOModSetup = []
            pyNewIOModSetup.append(request.extiosetup.type)
            pyNewIOModSetup.append(request.extiosetup.modName)
            if request.extiosetup.type == 0:
                pyRtuComm = (
                    request.extiosetup.rtuComm.baudrate,
                    request.extiosetup.rtuComm.datalength,
                    request.extiosetup.rtuComm.slaveId,
                    request.extiosetup.rtuComm.parity,
                    request.extiosetup.rtuComm.stopbit,
                )
                pyNewIOModSetup.append(pyRtuComm)
                if (
                    request.extiosetup.rtuComm.slaveId > 247
                    or request.extiosetup.rtuComm.slaveId < 0
                ):
                    grpc_logger().debug(
                        "slaveId %d out of range [%d,%d]"
                        % (request.extiosetup.rtuComm.slaveId, 0, 247)
                    )
                    return jaka_pb2.Response(
                        errcode=15, errormsg="Exception: parameter slaveId out of range"
                    )
            else:
                pyTcpComm = (
                    request.extiosetup.tcpComm.ipaddr,
                    request.extiosetup.tcpComm.port,
                    request.extiosetup.tcpComm.slaveId,
                )
                pyNewIOModSetup.append(pyTcpComm)

            pyPinSetup = []
            pyPinSetup.append(
                (
                    request.extiosetup.diSetup.startaddr,
                    request.extiosetup.diSetup.pinnum,
                )
            )
            pyPinSetup.append(
                (
                    request.extiosetup.doSetup.startaddr,
                    request.extiosetup.doSetup.pinnum,
                )
            )
            pyPinSetup.append(
                (
                    request.extiosetup.aiSetup.startaddr,
                    request.extiosetup.aiSetup.pinnum,
                )
            )
            pyPinSetup.append(
                (
                    request.extiosetup.aoSetup.startaddr,
                    request.extiosetup.aoSetup.pinnum,
                )
            )
            pyNewIOModSetup.append(pyPinSetup)
            pyNewIOModSetup.append(request.extiosetup.ExtIOModInfo)
            currExtIOSetups[request.intid] = tuple(pyNewIOModSetup)
            c.setup_extio(len(currExtIOSetups), currExtIOSetups)

            time.sleep(0.05)
            update_jkzuc_status()
            newExtIOConfigs = [s.extio_pinmap, s.extio_setup]
            self.updateExtIOName(oldExtIOConfigs, newExtIOConfigs)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def DeleteExtIOMod(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            # get current IO module setups
            update_jkzuc_status()
            extIOSetups = []
            oldExtIOConfigs = [s.extio_pinmap, s.extio_setup]
            for extioSetup in oldExtIOConfigs[1]:
                extIOSetups.append(extioSetup)

            # ensure the module exists
            if request.intid < 0 or request.intid >= len(extIOSetups):
                return jaka_pb2.Response(errcode=12, errormsg="Invalid arguments")

            del extIOSetups[request.intid]
            c.setup_extio(len(extIOSetups), extIOSetups)

            time.sleep(0.05)
            update_jkzuc_status()
            newExtIOConfigs = [s.extio_pinmap, s.extio_setup]
            self.updateExtIOName1(oldExtIOConfigs, newExtIOConfigs, request.intid)
            self.updateExtIOFunc_new(oldExtIOConfigs, newExtIOConfigs, request.intid)

            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def MoveExtIOMod(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            # get current IO module setups
            update_jkzuc_status()
            extIOSetups = []
            oldExtIOConfigs = [s.extio_pinmap, s.extio_setup]
            for extIOSetup in oldExtIOConfigs[1]:
                extIOSetups.append(extIOSetup)

            # ensure the module exists
            if (
                request.moveextioreq.currIndex < 0
                or request.moveextioreq.currIndex >= len(extIOSetups)
                or request.moveextioreq.tarIndex < 0
                or request.moveextioreq.tarIndex >= len(extIOSetups)
            ):
                return jaka_pb2.Response(errcode=12, errormsg="Invalid arguments")

            if request.moveextioreq.currIndex == request.moveextioreq.tarIndex:
                return jaka_pb2.Response(errcode=0)

            pyIOModSetup = extIOSetups.pop(request.moveextioreq.currIndex)
            extIOSetups.insert(request.moveextioreq.tarIndex, pyIOModSetup)
            c.setup_extio(len(extIOSetups), extIOSetups)
            time.sleep(0.05)
            newExtIOConfigs = [s.extio_pinmap, s.extio_setup]
            self.updateExtIOName_move(
                oldExtIOConfigs,
                newExtIOConfigs,
                request.moveextioreq.currIndex,
                request.moveextioreq.tarIndex,
            )
            self.updateExtIOFunc_move(
                oldExtIOConfigs,
                newExtIOConfigs,
                request.moveextioreq.currIndex,
                request.moveextioreq.tarIndex,
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetUpgradeInfo(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            ret, errmsg, robotUpgradeInfo = self.oneUpgrade.getVersion()
            if ret:
                return jaka_pb2.Response(errcode=ret, errormsg=errmsg)
            return jaka_pb2.Response(errcode=0, robotupgmsg=robotUpgradeInfo)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    @sim_mode_required(0)
    def ServoUpgrade(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.intid:  # 一键升级
                ret = self.oneUpgrade.triggerUpgrade(request, context)
                return ret
            ret, errmsg = firmwareUpgrade(request.str)  # 伺服或SCB升级
            if ret != 0:
                return jaka_pb2.Response(errcode=ret, errormsg=errmsg)
            threading.Timer(1, self.servo_upgrade_monitor).start()
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    @sim_mode_required(0)
    def SetJointBrakeRelease(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            brakeRelease = 0
            if request.servoData != 0:
                brakeRelease = 1
            c.set_brake_release(request.intid, brakeRelease)
            grpc_logger().debug(
                "[gRPC] joint {} break release: {}".format(request.intid, brakeRelease)
            )
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    @sim_mode_required(0)
    def SetJointMultiTurnStep(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            multiturnStep = 0
            if request.servoData > 0:
                multiturnStep = 1
            elif request.servoData < 0:
                multiturnStep = -1
            c.set_multiturn_step(request.intid, multiturnStep)
            grpc_logger().debug(
                "[gRPC] joint {} multiturn step: {}".format(
                    request.intid, multiturnStep
                )
            )
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetRobotModelVer(self, request, context):
        # 已废弃
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            model = s.robot(request.robot_index).model_data['model']
            grpc_logger().debug("robot model: {}".format(model))
            if model == 310:  # ZU3 1.0
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0310)
            elif model == 311:  # ZU3 1.1
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0311)
            elif model == 312:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0311)
            elif model == 313:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0311)
            elif model == 510:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.C_510)
            elif model == 511:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.C_511)
            elif model == 512:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_512)
            elif model == 513:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_512)
            elif model == 514:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_512)
            elif model == 515:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.PRO_515)
            # elif s.robot_model_data['model'] == 710:  #不再支持710
            #     return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0710)
            elif model == 720:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0720)
            elif model == 721:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0721)
            elif model == 722:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0722)
            elif model == 723:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0723)
            elif model == 724:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0724)
            elif model == 725:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0725)
            elif model == 726:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0726)
            elif model == 727:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_0727)
            elif model == 728:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_728_PRO)
            elif model == 729:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.C_710)
            elif model == 730:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.C_711)
            elif model == 731:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.PRO_731)
            elif model == 1210:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_1210)
            elif model == 1211:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_1211_PRO)
            elif model == 1212:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.C_1212)
            elif model == 1213:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.PRO_1213)
            elif model == 1610:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.PRO_1610)
            elif model == 1810:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_1810)
            elif model == 1811:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_1811_PRO)
            elif model == 1812:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_1811_PRO)
            elif model == 1813:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.PRO_1813)
            elif model == 2010:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_2010)
            elif model == 2011:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_2011_PRO)
            elif model == 2012:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_2011_PRO)
            elif model == 2020:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_2020)
            elif (
                model == 10115
                or model == 11115
                or model == 11315
            ):
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_10115)
            elif model == 11415:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_11415)
            elif model == 12015:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_11415)
            elif model == 12023:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_11415)
            elif model == 12120:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.ZU_12120)
            elif model == 20710:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.AT_20710)
            # 22001和22002是最早提供给ZY的机器人，后续改为20310和20311
            elif model == 22001:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.DU_20311)
            elif model == 22002:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.DU_20312)
            elif model == 20610:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.DU_20311)
            elif model == 20611:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.DU_20312)
            elif model == 33510:
                return jaka_pb2.Response(errcode=0, robotModelVer=jaka_pb2.MAX_33510)
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_INVALID_ROBOT_MODEL,
                errormsg="Invalid robot serial number",
            )
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetRobotModelData(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            robot_model_data = jaka_pb2.RobotModelData()
            robot_model_data.payload = s.robot_model_data["payload"]
            robot_model_data.maxTrajVel = s.robot_model_data["maxTrajVel"]
            robot_model_data.maxTcpVelLimit = 5000
            robot_model_data.maxTrajAcc = s.robot_model_data["maxTrajAcc"]
            robot_model_data.maxTrajVel_ori = s.robot_model_data["maxTrajVel_ori"]
            robot_model_data.maxTrajAcc_ori = s.robot_model_data["maxTrajAcc_ori"]
            for item in s.robot_model_data["minJointPosLimits"]:
                robot_model_data.minJointPosLimit.append(item)
            for item in s.robot_model_data["maxJointPosLimits"]:
                robot_model_data.maxJointPosLimit.append(item)
            for item in s.robot_model_data["maxJointVels"]:
                robot_model_data.maxJointVel.append(item)
            for item in s.robot_model_data["maxJointAccs"]:
                robot_model_data.maxJointAcc.append(item)
            for item in s.robot_model_data["motorSupplierId"]:
                robot_model_data.motorSupplierId.append(item)
            for item in s.robot_model_data["maxGearboxTorq"]:
                robot_model_data.maxGearboxTorq.append(item)

            robot_model_data.tioInfo.diNum = s.robot_model_data["tioInfo"][0]
            robot_model_data.tioInfo.doNum = s.robot_model_data["tioInfo"][1]
            robot_model_data.tioInfo.aiNum = s.robot_model_data["tioInfo"][2]
            robot_model_data.tioInfo.aoNum = s.robot_model_data["tioInfo"][3]

            robot_model_data.minMomentum = s.robot_model_data["minMomentum"]
            robot_model_data.maxMomentum = s.robot_model_data["maxMomentum"]
            robot_model_data.minRobotPower = s.robot_model_data["minRobotPower"]
            robot_model_data.maxRobotPower = s.robot_model_data["maxRobotPower"]
            robot_model_data.minReduceMomentum = s.robot_model_data["minReduceMomentum"]
            robot_model_data.maxReduceMomentum = s.robot_model_data["maxReduceMomentum"]
            robot_model_data.minReduceRobotPower = s.robot_model_data[
                "minReduceRobotPower"
            ]
            robot_model_data.maxReduceRobotPower = s.robot_model_data[
                "maxReduceRobotPower"
            ]
            robot_model_data.maxReduceTcpVelLimit = 250
            robot_model_data.maxReduceElbowVelLimit = 250
            return jaka_pb2.Response(errcode=0, robotModelData=robot_model_data)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetUserFrameId(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            self.ensure_mode(jkzuc.MODE_MANUAL)
            c.set_user_id(request.robot_index, request.intid)
            grpc_logger().debug("[gRPC] robot{} set user id: {}".format(request.robot_index, request.intid))
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetModbusSlaveId(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_mod_slaveid(request.intid)
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTorqueSensorComm(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.torqSensorCommType == 0:  # tcp/udp
                commInfo = (request.etherAddr.ipaddr, request.etherAddr.port)
                c.set_torqsensor_comm(0, commInfo)
            elif request.torqSensorCommType == 1:  # rs485
                # commInfo = (request.mbSlaveSettings.rtuSlaveSetting.baudrate, request.mbSlaveSettings.rtuSlaveSetting.databit,
                #             request.mbSlaveSettings.rtuSlaveSetting.stopbit, request.mbSlaveSettings.rtuSlaveSetting.parity)
                c.set_torqsensor_comm(1)
            else:
                return jaka_pb2.Response(errcode=-1, errormsg="Invalid arguments!")
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTorqueSensorComm(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            torqSnsrConfig = s.torqsensor[0]
            addr = jaka_pb2.EthAddr()
            if torqSnsrConfig[0] == 0:  # network
                addr.ipaddr = torqSnsrConfig[1][0]
                addr.port = torqSnsrConfig[1][1]
            return jaka_pb2.Response(
                errcode=0, torqSensorCommType=torqSnsrConfig[0], etherAddr=addr
            )
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTorqueSensorValue(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        try:
            value = s.torqsensor[1][2]
            torqueValue = jaka_pb2.CartPos()
            torqueValue.x = value[0]
            torqueValue.y = value[1]
            torqueValue.z = value[2]
            torqueValue.a = value[3]
            torqueValue.b = value[4]
            torqueValue.c = value[5]
            return jaka_pb2.Response(errcode=0, cartpos=torqueValue)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTorqueSensorPayload(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            m = request.payload.value
            p = (request.payload.xcom, request.payload.ycom, request.payload.zcom)
            c.set_torqsensor_payload(m, p)
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTorqueSensorPayload(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            torqSnsrConfig = s.torqsensor[0]
            snsrPayload = jaka_pb2.Payload()
            snsrPayload.value = torqSnsrConfig[2][0]
            snsrPayload.xcom = torqSnsrConfig[2][1][0]
            snsrPayload.ycom = torqSnsrConfig[2][1][1]
            snsrPayload.zcom = torqSnsrConfig[2][1][2]
            return jaka_pb2.Response(errcode=0, payload=snsrPayload)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetToolPayload(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            m = request.payload.value
            p = (request.payload.xcom, request.payload.ycom, request.payload.zcom)
            c.set_tool_payload(m, p)
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetToolPayload(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            toolPayLoadValue = s.toolPayLoad
            toolPayLoad = jaka_pb2.Payload()
            toolPayLoad.value = toolPayLoadValue[0]
            toolPayLoad.xcom = toolPayLoadValue[1][0]
            toolPayLoad.ycom = toolPayLoadValue[1][1]
            toolPayLoad.zcom = toolPayLoadValue[1][2]
            return jaka_pb2.Response(errcode=0, payload=toolPayLoad)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTorqueSensorSoftLimit(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            Fx = request.cartpos.x
            Fy = request.cartpos.y
            Fz = request.cartpos.z
            Mx = request.cartpos.a
            My = request.cartpos.b
            Mz = request.cartpos.c
            c.set_torque_sensor_soft_limit(Fx, Fy, Fz, Mx, My, Mz)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTorqueSensorFilter(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            torqueSensorFilter = request.flo
            c.set_torque_sensor_filter(torqueSensorFilter)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetCompliantType(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            compliantType = request.compliantTypeEnable.compliantType
            compliantEnable = request.compliantTypeEnable.compliantEnable
            c.set_compliant_type_enable(compliantType, compliantEnable)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetCompliantType(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            value = s.compliant_type_enable
            config = jaka_pb2.CompliantTypeEnable()
            config.compliantType = value[0]
            config.compliantEnable = value[1]
            return jaka_pb2.Response(errcode=0, compliantTypeEnable=config)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTorqueSensorFilter(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            torqueSensorFilterHz = s.torque_sensor_filter[0]
            return jaka_pb2.Response(errcode=0, flo=torqueSensorFilterHz)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTorqueSensorSoftLimit(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            value = s.torque_sensor_soft_limit
            grpc_logger().debug(s.torque_sensor_soft_limit)
            torquesensorsoftlimit = jaka_pb2.CartPos()
            torquesensorsoftlimit.x = value[0]
            torquesensorsoftlimit.y = value[1]
            torquesensorsoftlimit.z = value[2]
            torquesensorsoftlimit.a = value[3]
            torquesensorsoftlimit.b = value[4]
            torquesensorsoftlimit.c = value[5]
            return jaka_pb2.Response(errcode=0, cartpos=torquesensorsoftlimit)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetAdmitCtrlConfig(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            for value in request.admitCtrlConfig.constForce:
                axis = value.id - 1
                opt = value.opt
                ftUser = value.ftUser
                ftReboundFK = value.ftReboundFK
                ftConstant = value.ftConstant
                ftNnormalTrack = value.ftNnormalTrack
                ftconfig = (opt, ftUser, ftReboundFK, ftConstant, ftNnormalTrack)
                c.set_admitCtrl_config(axis, ftconfig, 1)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetAdmitCtrlConfig(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            value = s.admit_ctrl_config
            config = jaka_pb2.AdmitCtrlConfig()
            for i in range(0, 6):
                admitConfig = config.constForce.add()
                admitConfig.id = i + 1
                admitConfig.opt = value[i][0]
                admitConfig.ftUser = value[i][1]
                admitConfig.ftReboundFK = value[i][2]
                admitConfig.ftConstant = value[i][3]
                admitConfig.ftNnormalTrack = value[i][4]
            return jaka_pb2.Response(errcode=0, admitCtrlConfig=config)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTorqueSensorMode(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            grpc_logger().debug(request.intid)
            if request.intid != 0:
                c.set_torqsensor_mode(1)
            else:
                c.set_torqsensor_mode(0)
            grpc_logger().debug(
                "[gRPC] set torque sensor mode: {}".format(request.intid)
            )
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetAdmitControlOption(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            grpc_logger().debug("{}".format(request.admitCtrlOpt))
            option = []
            option.append(request.admitCtrlOpt.axisXEna)
            option.append(request.admitCtrlOpt.axisYEna)
            option.append(request.admitCtrlOpt.axisZEna)
            option.append(request.admitCtrlOpt.axisAEna)
            option.append(request.admitCtrlOpt.axisBEna)
            option.append(request.admitCtrlOpt.axisCEna)
            c.set_admit_opt(option)
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetAdmitControlOption(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            admitControlOpt = jaka_pb2.AdmitCtrlOpt()
            admitControlOpt.axisXEna = s.admit_axis_opt[0]
            admitControlOpt.axisYEna = s.admit_axis_opt[1]
            admitControlOpt.axisZEna = s.admit_axis_opt[2]
            admitControlOpt.axisAEna = s.admit_axis_opt[3]
            admitControlOpt.axisBEna = s.admit_axis_opt[4]
            admitControlOpt.axisCEna = s.admit_axis_opt[5]
            return jaka_pb2.Response(errcode=0, admitCtrlOpt=admitControlOpt)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def EnableAdmittanceCtrl(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            grpc_logger().debug(request.intid)
            if request.intid == 0:
                c.admitop_enable(0)
            else:
                c.admitop_enable(1)
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetModbusSlaveId(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            return jaka_pb2.Response(errcode=0, intid=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetModbusSlaveParam(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.mbSlaveSettings.type == 0:  # rtu
                commInfo = (
                    request.mbSlaveSettings.rtuSlaveSetting.slaveId,
                    request.mbSlaveSettings.rtuSlaveSetting.baudrate,
                    request.mbSlaveSettings.rtuSlaveSetting.databit,
                    request.mbSlaveSettings.rtuSlaveSetting.stopbit,
                    request.mbSlaveSettings.rtuSlaveSetting.parity,
                )
                c.setup_mb_slave(request.mbSlaveSettings.type, commInfo)
            elif request.mbSlaveSettings.type == 1:  # tcp
                grpc_logger().debug(request.mbSlaveSettings.tcpSlaveSetting.ipaddr)
                commInfo = (
                    request.mbSlaveSettings.tcpSlaveSetting.ipaddr,
                    request.mbSlaveSettings.tcpSlaveSetting.port,
                )
                c.setup_mb_slave(request.mbSlaveSettings.type, commInfo)
            else:
                return jaka_pb2.Response(errcode=-1, errormsg="Invalid arguments!")

            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetModbusSlaveParam(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            mbSlaveCommType, mbSlaveCommInfo = s.slave_config
            mbslave = jaka_pb2.MBSlaveSetting()
            mbslave.type = mbSlaveCommType
            if mbSlaveCommType == 0:
                mbslave.rtuSlaveSetting.slaveId = mbSlaveCommInfo["slaveId"]
                mbslave.rtuSlaveSetting.baudrate = mbSlaveCommInfo["baudrate"]
                mbslave.rtuSlaveSetting.databit = mbSlaveCommInfo["databit"]
                mbslave.rtuSlaveSetting.stopbit = mbSlaveCommInfo["stopbit"]
                mbslave.rtuSlaveSetting.parity = mbSlaveCommInfo["parity"]
            elif mbSlaveCommType == 1:  # tcp
                mbslave.tcpSlaveSetting.ipaddr = mbSlaveCommInfo["ipaddr"]
                mbslave.tcpSlaveSetting.port = mbSlaveCommInfo["port"]
            else:
                return jaka_pb2.Response(
                    errcode=-1, errormsg="Invalid modbus settings!"
                )

            return jaka_pb2.Response(errcode=0, mbSlaveSettings=mbslave)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetCustomPose(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.intid <= 2:
                joints = (
                    request.jointpos.joint1,
                    request.jointpos.joint2,
                    request.jointpos.joint3,
                    request.jointpos.joint4,
                    request.jointpos.joint5,
                    request.jointpos.joint6,
                    request.jointpos.joint7,
                )
                c.set_custom_pose(request.robot_index, request.intid, joints)
            elif request.intid == 3:
                if request.flo < 0.001 or request.flo > 3:
                    return jaka_pb2.Response(
                        errcode=-1,
                        errormsg="Invalid arguments!, flo should be within 0.001 and 3",
                    )
                c.set_custom_pose(request.robot_index, request.intid, request.flo)
            else:
                return jaka_pb2.Response(errcode=-1, errormsg="Invalid arguments!")
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetCustomPose(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            jointPos = (0, 0, 0, 0, 0, 0, 0)
            joints = jaka_pb2.JointPos()
            rb = s.robot(request.robot_index)
            if request.intid <= 2 and request.intid >= 0:
                if request.intid == 0:  # shrunken pose
                    jointPos = rb.safe_pose_config["shrunken_pose"]
                elif request.intid == 1:  # stretched pose
                    jointPos = rb.safe_pose_config["stretched_pose"]
                elif request.intid == 2:  # initial pose
                    jointPos = rb.safe_pose_config["initial_pose"]
                joints.joint1 = jointPos[0]
                joints.joint2 = jointPos[1]
                joints.joint3 = jointPos[2]
                joints.joint4 = jointPos[3]
                joints.joint5 = jointPos[4]
                joints.joint6 = jointPos[5]
                joints.joint7 = jointPos[6]
                return jaka_pb2.Response(
                    errcode=0, intid=request.intid, jointpos=joints
                )
            elif request.intid == 3:
                postol = rb.safe_pose_config["initial_pose_tol"]
                return jaka_pb2.Response(errcode=0, intid=request.intid, flo=postol)
            else:
                return jaka_pb2.Response(errcode=-1, errormsg="Invalid arguments!")
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetSystemMonitorData(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            robot_joint_num = s.robot_joint_num
            monitor_data = s.monitor_data
            joint_pos_limted = s.is_joint_pos_limited
            joint_vel_limted = s.is_joint_vel_limited
            monitorData = jaka_pb2.SystemMonitoringData()
            monitorData.cabTemperature = monitor_data[2]
            monitorData.robotAveragePower = monitor_data[3]
            monitorData.robotAverageCurrent = monitor_data[4]
            for i in range(0, robot_joint_num):
                rawJointMonitorData = monitor_data[5][i]
                jointData = monitorData.jointMonitorData.add()
                jointData.instCurrent = abs(
                    rawJointMonitorData[0]
                )  # absolute current value
                jointData.instVoltage = rawJointMonitorData[1]
                jointData.instTemperature = rawJointMonitorData[2]
                jointData.isJointPosLimit = joint_pos_limted[i]
                jointData.isJointVelLimit = joint_vel_limted[i]
                if self.use_cmd_torq == 0:
                    jointData.instJointTorq = rawJointMonitorData[9]
                else:
                    jointData.instJointTorq = rawJointMonitorData[10]
            monitorData.robotVoltage = monitor_data[9]
            del monitor_data
            return jaka_pb2.Response(errcode=0, sysMtrData=monitorData)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ExportZucSettings(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            # create folder /tmp/XXXXzucsettings
            pre_cwd = os.getcwd()
            dirname = tempfile.mkdtemp("zucsettings")
            file_path = dirname + "/zucsettings/"
            os.makedirs(file_path)

            # cp usersettings.ini, aliasname.ini, extiosetup.xml
            exportOpt = {}
            exportOpt["usersettings.ini"] = request.configOpt.controllerConfig
            exportOpt["usersettings_template.ini"] = request.configOpt.controllerConfig
            exportOpt["aliasname.ini"] = request.configOpt.ioNameConfig
            exportOpt["aliasname_template.ini"] = request.configOpt.ioNameConfig
            exportOpt["extiosetup.xml"] = request.configOpt.extIOConfig
            exportOpt["jaka_user.var"] = request.configOpt.sysVarConfig
            exportOpt["safezone.xml"] = request.configOpt.safezoneConfig
            for fileName, bExport in exportOpt.items():
                filepath = jkzuc.jkroot_dir + "/configs/JAKA/" + fileName
                if os.path.exists(filepath) and bExport:
                    shutil.copyfile(filepath, file_path + fileName)

            # create a version file
            with open(file_path + "version", "a") as fd:
                fd.write(jkzuc.controller_version)
                fd.close()

            # create a export info file
            update_jkzuc_status()
            exportInfo = MyConfigParser()
            if not exportInfo.has_section("EXPORT_INFO"):
                exportInfo.add_section("EXPORT_INFO")
            exportInfo.set("EXPORT_INFO", "CABTYPE", str(s.cab_type))
            # scb major, scb minor
            scb_major = s.monitor_data[0]
            scb_minor = s.monitor_data[1]
            scb_version = "%c%c_%c%c" % (
                (scb_major >> 8) & 0xFF,
                scb_major & 0xFF,
                (scb_minor >> 8) & 0xFF,
                scb_minor & 0xFF,
            )
            exportInfo.set("EXPORT_INFO", "SCBVER", scb_version)

            servo_ver, _ = tool_function.getServoVerStr(s.servo_version)
            exportInfo.set("EXPORT_INFO", "SERVO_VER", servo_ver)
            with codecs.open(
                file_path + "exportinfo.ini", "w+", encoding="utf-8"
            ) as fd:
                exportInfo.write(fd)

            # tar to a package
            os.chdir(dirname)
            os.system("tar -zcvf ../zucsettings.tar.gz " + "./zucsettings")

            # pass via stream
            file_data = jaka_pb2.FileData()
            with open("/tmp/zucsettings.tar.gz", "rb") as fd:
                file_data.filename = "zucsettings.tar.gz"
                for line in fd.readlines():
                    file_data.data += line
                file_data.digest = tool_function.getMD5(file_data.data)

            # remove the temp files
            os.chdir(pre_cwd)
            shutil.rmtree(dirname)
            os.remove("/tmp/zucsettings.tar.gz")
            return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK, fileData=file_data)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_EXCEPT,
                errormsg="Exception:ExportZucSettings failed",
            )

    def ImportZucSettings(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            # CAB3.0 暂时不支持配置导入
            resp = jaka_pb2.Response(
                errcode=0x002B, errormsg="controller version doesnot match."
            )
            return resp
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(
                errcode=0x002B, errormsg="Exception:ImportZucSettings failed"
            )

    def StartIdentifyDynamics(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.start_identify_dynamics()
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def StopIdentifyDynamics(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.stop_identify_dynamics()
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetDynamicsIdentifyResult(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            file_data = jaka_pb2.FileData()
            with open(jkzuc.jkroot_dir + "/scripts/log/identify_res.txt", "rb") as fd:
                file_data.filename = jkzuc.jkroot_dir + "/scripts/log/identify_res.txt"
                for line in fd.readlines():
                    file_data.data += line
                file_data.digest = tool_function.getMD5(file_data.data)
            return jaka_pb2.Response(errcode=0, fileData=file_data)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetIdentifyFrictionStartPos(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_identify_friction_start_pos(
                request.intid,
                request.jointpos.joint1,
                request.jointpos.joint2,
                request.jointpos.joint3,
                request.jointpos.joint4,
                request.jointpos.joint5,
                request.jointpos.joint6,
                request.jointpos.joint7,
            )
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetIdentifyFrictionEndPos(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_identify_friction_end_pos(
                request.intid,
                request.jointpos.joint1,
                request.jointpos.joint2,
                request.jointpos.joint3,
                request.jointpos.joint4,
                request.jointpos.joint5,
                request.jointpos.joint6,
                request.jointpos.joint7,
            )
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def StartIdentifyFriction(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.start_identify_friction(request.intid)
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def StopIdentifyFriction(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.stop_identify_friction(request.intid)
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetServoIdentifyPara(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_servo_dynamics_para()
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetServoFrictionPara(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_servo_friction()
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetFrictionIdentifyResult(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            file_data = jaka_pb2.FileData()
            jnt_id = request.intid
            file_name = jkzuc.jkroot_dir + "/scripts/log/identify_res_%d.txt" % (jnt_id)
            with open(file_name, "rb") as fd:
                file_data.filename = file_name
                file_data.data = fd.readline()
                file_data.digest = tool_function.getMD5(file_data.data)
            return jaka_pb2.Response(errcode=0, fileData=file_data)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetPayloadIdentifyTrajStartPos(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            startJoints = (
                request.jointTraj.startJntPos.joint1,
                request.jointTraj.startJntPos.joint2,
                request.jointTraj.startJntPos.joint3,
                request.jointTraj.startJntPos.joint4,
                request.jointTraj.startJntPos.joint5,
                request.jointTraj.startJntPos.joint6,
                request.jointTraj.startJntPos.joint7,
            )
            c.set_identify_traj_startpos(request.intid, startJoints)
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetPayloadIdentifyTrajEndPos(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            endJoints = (
                request.jointTraj.endJntPos.joint1,
                request.jointTraj.endJntPos.joint2,
                request.jointTraj.endJntPos.joint3,
                request.jointTraj.endJntPos.joint4,
                request.jointTraj.endJntPos.joint5,
                request.jointTraj.endJntPos.joint6,
                request.jointTraj.endJntPos.joint7,
            )
            c.set_identify_traj_endpos(request.intid, endJoints)
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetPayloadIdentifyTraj(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            jntTraj = jaka_pb2.JointTraj()
            if request.intid >= 0 and request.intid < 4:
                traj = s.identify_traj[request.intid]
                jntTraj.startJntPos.joint1 = traj["startJntPos"][0]
                jntTraj.startJntPos.joint2 = traj["startJntPos"][1]
                jntTraj.startJntPos.joint3 = traj["startJntPos"][2]
                jntTraj.startJntPos.joint4 = traj["startJntPos"][3]
                jntTraj.startJntPos.joint5 = traj["startJntPos"][4]
                jntTraj.startJntPos.joint6 = traj["startJntPos"][5]
                jntTraj.startJntPos.joint7 = traj["startJntPos"][6]
                jntTraj.endJntPos.joint1 = traj["endJntPos"][0]
                jntTraj.endJntPos.joint2 = traj["endJntPos"][1]
                jntTraj.endJntPos.joint3 = traj["endJntPos"][2]
                jntTraj.endJntPos.joint4 = traj["endJntPos"][3]
                jntTraj.endJntPos.joint5 = traj["endJntPos"][4]
                jntTraj.endJntPos.joint6 = traj["endJntPos"][5]
                jntTraj.endJntPos.joint7 = traj["endJntPos"][6]
            return jaka_pb2.Response(errcode=0, jointTraj=jntTraj)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def StartPayloadIdentifyTraj(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.start_identify_traj(
                request.identifyOpt.trajIndex, request.identifyOpt.withPayload
            )
            grpc_logger().debug("[gRPC] start payload identification")
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetPayloadIdentifyResult(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            res = c.get_identify_result()
            if res:
                load = jaka_pb2.Payload()
                load.value = res[0]
                load.xcom = res[1][0]
                load.ycom = res[1][1]
                load.zcom = res[1][2]
                err = res[2]
                if err < 0:
                    load.value = 0
                    load.xcom = 0
                    load.ycom = 0
                    load.zcom = 0
                    return jaka_pb2.Response(
                        errcode=0, errormsg="Failed to get identification result"
                    )
                grpc_logger().debug(load)
                return jaka_pb2.Response(errcode=0, payload=load)
            else:
                return jaka_pb2.Response(
                    errcode=0, errormsg="Failed to get identification result"
                )
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def RecordTorqSensorIdentifyData(self, endJoints):
        try:
            global IDENTIFY_COMPLETE
            global MASS
            global CENTROID
            self.ensure_mode(jkzuc.MODE_AUTO)
            c.jog_movj(
                0x10,
                endJoints[0],
                endJoints[1],
                endJoints[2],
                endJoints[3],
                endJoints[4],
                endJoints[5],
                endJoints[6],
                3,
                90,
            )  # 30,90
            update_jkzuc_status()
            move_flag = s.inpos
            with open("/home/jakauser/tooldata.txt", "w") as file:
                while not move_flag:
                    update_jkzuc_status()
                    move_flag = s.inpos
                    value = list(s.actual_position[3:6] + s.torqsensor[1][3])
                    string = list(map(lambda x: str(x), value))
                    value_sequence = " ".join(list(string))
                    time.sleep(0.015)
                    file.writelines(value_sequence + "\n")

            res = c.get_payload_auto_identify_result()
            self.ensure_mode(jkzuc.MODE_MANUAL)
            if res:
                MASS = res[0]
                CENTROID = (res[1][0], res[1][1], res[1][2])
                IDENTIFY_COMPLETE = 0  # 0 辨识完成
                return True
            else:
                IDENTIFY_COMPLETE = 2  # 0 辨识失败
                return False
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def StartTorqSensorPayloadIdentify(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            startJoints = (
                request.jointTraj.startJntPos.joint1,
                request.jointTraj.startJntPos.joint2,
                request.jointTraj.startJntPos.joint3,
                request.jointTraj.startJntPos.joint4,
                request.jointTraj.startJntPos.joint5,
                request.jointTraj.startJntPos.joint6,
                request.jointTraj.startJntPos.joint7,
            )
            endJoints = (
                request.jointTraj.endJntPos.joint1,
                request.jointTraj.endJntPos.joint2,
                request.jointTraj.endJntPos.joint3,
                request.jointTraj.endJntPos.joint4,
                request.jointTraj.endJntPos.joint5,
                request.jointTraj.endJntPos.joint6,
                request.jointTraj.endJntPos.joint7,
            )
            # res = c.get_payload_auto_identify_result(startJoints,endJoints)
            t = threading.Thread(
                target=self.RecordTorqSensorIdentifyData, args=(endJoints,)
            )
            t.start()
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTorqSensorPayloadIdentifyResult(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        global IDENTIFY_COMPLETE
        global MASS
        global CENTROID
        try:
            toolPayLoad = jaka_pb2.Payload()
            toolPayLoad.value = MASS
            toolPayLoad.xcom = CENTROID[0]
            toolPayLoad.ycom = CENTROID[1]
            toolPayLoad.zcom = CENTROID[2]
            IDENTIFY_COMPLETE = 1
            return jaka_pb2.Response(errcode=0, payload=toolPayLoad)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTorqSensorIdentifyStaus(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        global IDENTIFY_COMPLETE
        try:
            return jaka_pb2.Response(errcode=0, intid=IDENTIFY_COMPLETE)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ResetPayloadIdentifyStatus(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.reset_identify_status(request.intid)
            return jaka_pb2.Response(errcode=0)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetPayloadIdentifyStatus(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            status = jaka_pb2.LoadIdentifyStatus()
            for i in range(0, 3):
                noLoadStatVal = s.load_identify_stat["noLoad"][i]
                if noLoadStatVal == -1:
                    status.noLoadStatus.append(
                        jaka_pb2.LoadIdentifyStatus.IDENTIFY_ERROR
                    )
                else:
                    status.noLoadStatus.append(noLoadStatVal)

                onLoadStatVal = s.load_identify_stat["onLoad"][i]
                if onLoadStatVal == -1:
                    status.onLoadStatus.append(
                        jaka_pb2.LoadIdentifyStatus.IDENTIFY_ERROR
                    )
                else:
                    status.onLoadStatus.append(onLoadStatVal)

            return jaka_pb2.Response(errcode=0, loadIdentifyStat=status)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetDynamicsIdentifyStatus(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:

            s.poll()
            t = ctypes.py_object * 1
            my_tuple = t()
            my_tuple = s.dynamics_identify_stat
            dynamicsIdentify_stat = my_tuple[0]
            grpc_logger().debug("Dyanmcis State: {}".format(dynamicsIdentify_stat))
            return jaka_pb2.Response(errcode=0, dynamicstat=dynamicsIdentify_stat)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call11 failed")

    def GetFrictionIdentifyStatus(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            s.poll()
            t = ctypes.py_object * 1
            my_tuple = t()
            my_tuple = s.friction_identify_stat
            frictionIdentify_stat = my_tuple[0]
            grpc_logger().debug("Dyanmcis State: {}".format(frictionIdentify_stat))
            return jaka_pb2.Response(errcode=0, frictionstat=frictionIdentify_stat)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call11 failed")

    def GetDynamicsIdentifyStartPos(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            s.poll()
            jointPos = (0, 0, 0, 0, 0, 0, 0)
            joints = jaka_pb2.JointPos()
            dynamicsIdentifyStartPos = s.dynamics_identify_start_pos
            joints.joint1 = dynamicsIdentifyStartPos[0]
            joints.joint2 = dynamicsIdentifyStartPos[1]
            joints.joint3 = dynamicsIdentifyStartPos[2]
            joints.joint4 = dynamicsIdentifyStartPos[3]
            joints.joint5 = dynamicsIdentifyStartPos[4]
            joints.joint6 = dynamicsIdentifyStartPos[5]
            joints.joint7 = dynamicsIdentifyStartPos[6]
            grpc_logger().debug(
                "Dyanmcis StartPos: {}".format(dynamicsIdentifyStartPos)
            )

            return jaka_pb2.Response(errcode=0, jointpos=joints)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def ProgramStep(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            with open(s.file, "r") as fd:
                if "usr/etc/jkzuc/scripts/track" in fd.read():
                    return jaka_pb2.Response(
                        errcode=jaka_pb2.RPC_PROGSTEP_UNSUPPORTED_COMMAND, intid=-1
                    )
            start_line = s.task_executing_line
            end_line = -1
            start_time = time.time()
            repeatCounter = 0
            """
             FIXME：此处会存在从非运动指令到运动指令时，需要单步两次的问题，后续修复。
             step放到while内则会导致，当脚本解析错误时，在python层无法获取到错误导致多发出一次step从而导致再一次进入run模式（APP报错后程序显示运行状态），
             此时若再次进入调试模式指针会指到的不是第一行
            """
            while start_line >= end_line and repeatCounter <= 1:
                self.ensure_mode(jkzuc.MODE_AUTO)
                c.auto(jkzuc.AUTO_STEP)
                time.sleep(0.05)
                update_jkzuc_status()
                if s.task_mode != jkzuc.MODE_AUTO:
                    start_line = -1
                    break
                end_line = s.task_executing_line
                if end_line == 0:
                    break
                end_time = time.time()
                if start_line == end_line:
                    repeatCounter += 1
                if end_time - start_time > 0.1:
                    break
            return jaka_pb2.Response(errcode=0, intid=start_line)
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def WifiModuleConfig(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        return jaka_pb2.Response(
            errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
        )

    def AutoIdentifyInitPoint(self, request, context):
        try:
            # update_jkzuc_status()
            # s.auto_identify_init_point
            pass
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def AutoIdentifyToolPayload(self, request, context):
        try:
            update_jkzuc_status()
            # value = s.auto_identify_torque_sensor_payload
            toolPayLoad = jaka_pb2.Payload()
            toolPayLoad.value = 0
            toolPayLoad.xcom = 0
            toolPayLoad.ycom = 0
            toolPayLoad.zcom = 0
            return jaka_pb2.Response(errcode=0, payload=toolPayLoad)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTorsensorBrand(self, request, context):
        try:
            c.set_torqsensor_brand(request.intid)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTorsensorBrand(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            torqsensorBrand = int(s.torque_sensor_brand[0])
            return jaka_pb2.Response(errcode=0, intid=torqsensorBrand)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetScriptAutoBackup(self, request, context):
        try:
            c.set_script_backup(request.intid, request.flo)
            return jaka_pb2.Response(errcode=0, intid=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetScriptAutoBackup(self, request, context):
        try:
            update_jkzuc_status()
            return jaka_pb2.Response(
                errcode=0, intid=s.backup_setting[0], flo=s.backup_setting[1]
            )
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetupDigitalChannel(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if s.cab_type == 3 and request.intid >= 0 and request.intid < 7:
                # 1 for digital channel
                c.setup_io_channel(1, request.intid, request.digChannelSetting)
                c.wait_complete()
                time.sleep(0.2)
                update_jkzuc_status()
                res = jaka_pb2.Response()
                res.intid = request.intid
                res.digChnSettings.append(s.scb_digchn_setting[request.intid])
                grpc_logger().debug(
                    "[gRPC] setup digital channel: {} {}".format(
                        request.intid, request.digChannelSetting
                    )
                )
                return res
            return jaka_pb2.Response(errcode=-1, errormsg="Invalid arguments!")
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetDigitalChannelSetting(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            res = jaka_pb2.Response()
            if s.cab_type == 3:
                scbDigChnSettings = s.scb_digchn_setting
                for chnSetting in scbDigChnSettings:
                    res.digChnSettings.append(chnSetting)
            return res
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetBRStartVoltage(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            grpc_logger().debug(
                "[gRPC] set braking resistor vol: {}".format(request.intid)
            )
            if request.intid >= 20 and request.intid <= 63:
                c.set_br_start_vol(request.intid)
                return jaka_pb2.Response(errcode=0)
            else:
                return jaka_pb2.Response(errcode=-1, errormsg="Invalid arguments!")
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetBRStartVoltage(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            res = jaka_pb2.Response()
            if s.cab_type == 3:
                res.intid = s.scb_br_start_vol
            return res
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetDhcpOrStaticIP(self, request, context):
        """
        设置网络IP
        :param request:
                dhcporstaticip: type: 1:static, 2:dhcp
                                lanid: 1~4
                                ip: ip address, only for static
                                netmask: netmask, only for static
                                gateway: gateway, only for static

        :param context:
        :note: 1. 对于较老的app，其仅能设置LAN1。对于新的app，其可以指定设置LAN1~4
        """
        grpc_logger().info("set network : {}".format(request))
        update_jkzuc_status()

        # FIXME:这里应该检查程序是否在运行而不使能
        if s.enabled:
            grpc_logger().warn("Cannot set network when robot is enabled")
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="Please servo off the robot first"
            )

        try:
            filename = "/etc/systemd/network/primary.network"
            ip_type = request.dhcporstaticip.type

            # net_type = "enp3s0"
            face = netifaces.interfaces()
            net_name = ""
            secondary_net_name = ""
            # minicab
            if s.cab_type == 3:
                # net_name = "enp4s0"  # minicab 1.0 hengbang control board
                if "enp1s0" in face:
                    net_name = "enp1s0"  # minicab 1.1 hengbang control board
                    secondary_net_name = "enp4s0"
                else:
                    return jaka_pb2.Response(
                        errcode=jaka_pb2.RPC_EXCEPT,
                        errormsg="MiniCAB v1.0 does not support network setting",
                    )

                # in minicab if network segment conflict return error
                if len(face) > 2:  # minicab 1.1
                    facetmp = face
                    facetmp.remove("lo")
                    facetmp.remove(net_name)
                    otherIpAddr = netifaces.ifaddresses(facetmp[0])[netifaces.AF_INET][
                        0
                    ]["addr"]
                    ipAddress = str((request.dhcporstaticip.ip))

                    if otherIpAddr[:otherIpAddr.rfind('.')] == ipAddress[:ipAddress.rfind('.')]:
                        return jaka_pb2.Response(errcode=jaka_pb2.RPC_INVALID_ARGS, errormsg='invalid parameters')
            else:
                net_name = "enp3s0"  # hengbang control board
                secondary_net_name = "enp2s0"
                if "enp4s0" in face:
                    net_name = "enp2s0"  # yanhua control board
                    secondary_net_name = "enp4s0"
            grpc_logger().debug("type: {}".format(ip_type))
            config = MyConfigParser()
            config.add_section("Match")
            config.set("Match", "Name", net_name)
            if ip_type == 1:
                # check ip syntax error
                if (
                    len(request.dhcporstaticip.ip) == 0
                    or len(request.dhcporstaticip.netmask) == 0
                ):
                    return jaka_pb2.Response(
                        errcode=jaka_pb2.RPC_INVALID_ARGS,
                        errormsg="Exception: invalid parameters",
                    )
                # check ip settings available
                pingback = os.system("ping -c 1 -w 1 %s" % request.dhcporstaticip.ip)
                if pingback == 0:  # has a device with the specified ip
                    return jaka_pb2.Response(
                        errcode=jaka_pb2.RPC_IP_EXISTED,
                        errormsg="Exception: ip address existed",
                    )
                # write the settings
                config.add_section("Network")

                netmaskbits = sum(
                    bin(int(x)).count("1")
                    for x in request.dhcporstaticip.netmask.split(".")
                )
                netmaskfb = socket.inet_ntoa(
                    struct.pack("!I", (1 << 32) - (1 << (32 - netmaskbits)))
                )
                if netmaskfb != request.dhcporstaticip.netmask:
                    return jaka_pb2.Response(
                        errcode=jaka_pb2.RPC_INVALID_ARGS,
                        errormsg="Exception: invalid parameters",
                    )

                if secondary_net_name in face:
                    secondary_net_ip = netifaces.ifaddresses(secondary_net_name)[
                        netifaces.AF_INET
                    ][0]["addr"]
                    secondary_net_mask = netifaces.ifaddresses(secondary_net_name)[
                        netifaces.AF_INET
                    ][0]["netmask"]
                    ## 检查 两个网卡的IP是否在同网段 或 子网段
                    if -1 == check_net(
                        secondary_net_ip,
                        secondary_net_mask,
                        request.dhcporstaticip.ip,
                        request.dhcporstaticip.netmask,
                    ):
                        return jaka_pb2.Response(
                            errcode=jaka_pb2.RPC_INVALID_ARGS,
                            errormsg="Exception: invalid parameters",
                        )

                ipaddress = request.dhcporstaticip.ip + "/" + str(netmaskbits)
                config.set("Network", "Address", ipaddress)
                # config.set("Network", "Netmask", request.dhcporstaticip.netmask)
                if request.dhcporstaticip.gateway != "":
                    config.set("Network", "Gateway", request.dhcporstaticip.gateway)
                grpc_logger().debug(
                    "[gRPC] setup static ip: cab {} {} {}".format(
                        s.cab_type, ipaddress, request.dhcporstaticip.gateway
                    )
                )
            else:
                config.add_section("Network")
                config.set("Network", "DHCP", "ipv4")
                config.add_section("DHCP")
                config.set("DHCP", "ClientIdentifier", "mac")
            with open(filename, "w+") as fd:
                config.write(fd)
            self.sync_path(filename)
            # let the settings take effect
            restart_systemd_networkd()
            return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
            )

    def convert_subnet_mask(self, subnet_prefix):
        subnet_mask = ""
        for i in range(4):
            if subnet_prefix >= 8:
                subnet_mask += "255."
                subnet_prefix -= 8
            else:
                subnet_mask += str(256 - 2 ** (8 - subnet_prefix)) + "."
                subnet_prefix = 0
        return subnet_mask.rstrip(".")

    def GetDhcpOrStaticIP(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            dhcp_staicIP = jaka_pb2.DhcpOrStaticIP()
            config = MyConfigParser()
            config.read("/etc/systemd/network/primary.network")
            if config.has_section("DHCP"):
                dhcp_staicIP.type = 0
            else:
                dhcp_staicIP.type = 1
            net_name = ""
            ipaddr = ""
            netmask = ""
            gateway = ""
            cabIntf = 0

            # 对于获取不到的情况，则采用APIPA的地址范围作为默认值
            DEFAULT_IPADDR = "169.254.255.254"  # 从169.254.0.1到169.254.255.254
            DEFAULT_NETMASK = "255.255.0.0"
            DEFAULT_GATEWAY = "0.0.0.0"

            try:
                face = netifaces.interfaces()
                if s.cab_type == 3:
                    net_name = "enp4s0"  # minicab 1.0 hengbang control board
                    if "enp1s0" in face:
                        net_name = "enp1s0"  # minicab 1.1 hengbang control board
                else:
                    net_name = "enp3s0"  # hengbang control board
                    if "enp4s0" in face:
                        net_name = "enp2s0"  # yanhua control board
                for intf in face:
                    if "enp" in intf:
                        cabIntf = 1  # 存在真机网口
                if cabIntf == 0:  # 获取虚拟机网口名称
                    net_name = config.get("Match", "Name")
            except Exception:
                grpc_logger().error(traceback.format_exc())
                try:
                    net_name = config.get("Match", "Name")
                except Exception:
                    grpc_logger().error(traceback.format_exc())
                    net_name = ""

            if net_name != "":
                try:
                    info = netifaces.ifaddresses(net_name)[netifaces.AF_INET]
                    ipaddr = info[0]["addr"]
                    netmask = info[0]["netmask"]
                except Exception:
                    grpc_logger().error(traceback.format_exc())
                    if dhcp_staicIP.type == 0:
                        ipaddr = DEFAULT_IPADDR
                        netmask = DEFAULT_NETMASK
                    else:
                        try:
                            ip_address = config["Network"]["Address"]
                            if "/" in ip_address:
                                ipaddr, subnet_prefix = ip_address.split("/")
                                netmask = self.convert_subnet_mask(int(subnet_prefix))
                            else:
                                ipaddr = config.get("Network", "Address")
                                netmask = config.get("Network", "Netmask")
                        except Exception:
                            grpc_logger().error(traceback.format_exc())
                            ipaddr = DEFAULT_IPADDR
                            netmask = DEFAULT_NETMASK
                try:
                    info = netifaces.gateways()[netifaces.AF_INET]
                    for i in info:
                        if i[1] == net_name:
                            gateway = i[0]
                except Exception:
                    grpc_logger().error(traceback.format_exc())
                    if dhcp_staicIP.type == 0:
                        gateway = DEFAULT_GATEWAY
                    else:
                        try:
                            gateway = config.get("Network", "Gateway")
                        except Exception:
                            grpc_logger().error(traceback.format_exc())
                            gateway = DEFAULT_GATEWAY

            if ipaddr == "":
                ipaddr = DEFAULT_IPADDR  # default loopback ip address
            if netmask == "":
                netmask = DEFAULT_NETMASK
            if gateway == "":
                gateway = DEFAULT_GATEWAY
            dhcp_staicIP.ip = ipaddr
            dhcp_staicIP.netmask = netmask
            dhcp_staicIP.gateway = gateway
            return jaka_pb2.Response(errcode=0, dhcporstaticip=dhcp_staicIP)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetCABVersion(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            res = jaka_pb2.Response()
            if s.cab_type >= 1 and s.cab_type <= 4:
                res.cabType = s.cab_type
            else:
                res.cabType = jaka_pb2.CAB_VER_UNKNOWN
            return res
        except:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def _backup_script_process(self, old_script_path, new_script_path):
        try:
            scripts_path = "/usr/etc/jkzuc/scripts/"
            shutil.copytree(
                scripts_path + old_script_path, scripts_path + new_script_path
            )
            c.set_grpc_errcode(
                jkzuc.GRPC_RESPONSE_INFO,
                0x0F2004,
                'backup successed. # {"fixed_key":["'
                + old_script_path
                + '","'
                + new_script_path
                + '"]}',
                0xFF,
                1,
            )
            time.sleep(0.2)
        except Exception as e:
            print(traceback.format_exc())
            c.set_grpc_errcode(
                jkzuc.GRPC_RESPONSE_ERROR,
                0x0F2005,
                'backup failed. # {"fixed_key":["'
                + old_script_path
                + '","'
                + new_script_path
                + '"]}',
                0xFF,
                1,
            )

    def BackupScript(self, request, context):
        try:
            oldScriptPath = request.str
            newScriptPath = oldScriptPath + datetime.now().strftime(".%y%m%d%H%M%S")
            scripts_path = jkzuc.jkroot_dir + "/scripts/"
            real_path = os.path.realpath(oldScriptPath)
            if (scripts_path in real_path) == False:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_PERMISSION_DENY, errormsg="permission denied"
                )
            newScriptPath = oldScriptPath + datetime.now().strftime(".%y%m%d%H%M%S")
            # shutil.copytree(oldScriptPath, newScriptPath)
            p = multiprocessing.Process(
                target=self._backup_script_process, args=(oldScriptPath, newScriptPath)
            )
            p.start()
            # p.join()  #不会阻塞到进程p运行结束
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            c.set_grpc_errcode(
                jkzuc.GRPC_RESPONSE_ERROR,
                0x0F2005,
                'backup failed. # {"fixed_key":["'
                + oldScriptPath
                + '","'
                + newScriptPath
                + '"]}',
                0xFF,
                1,
            )
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetAdmittanceCtrlFrame(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_admittance_ctrl_frame(request.intid)
            grpc_logger().debug("request.intid: {}".format(request.intid))
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetAdmittanceCtrlFrame(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            ftFrame = s.admittance_ctrl_frame[0]
            return jaka_pb2.Response(errcode=0, intid=ftFrame)
        except Exception:

            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def DisableForceControl(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.disable_force_control(request.intid)
            grpc_logger().debug("[gRPC] disable force control")
            return jaka_pb2.Response(errcode=0)
        except:
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTrajTrackConfig(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        if (
            request.trajtrackconfig.xyz_interval < 0.1
            or request.trajtrackconfig.xyz_interval > 10
        ):
            return jaka_pb2.Response(
                errcode=-1,
                errormsg="Para Error! xyz_interval should be within 0.1 and 10",
            )
        if (
            request.trajtrackconfig.rpy_interval < 0.1
            or request.trajtrackconfig.rpy_interval > 10
        ):
            return jaka_pb2.Response(
                errcode=-1,
                errormsg="Para Error! rpy_interval should be within 0.1 and 10",
            )

        update_jkzuc_status()
        # 程序运行时不允许修改轨迹配置文件
        if s.interp_state != jkzuc.INTERP_IDLE:
            grpc_logger().debug("Program is running, can not setTrajTrackConfig ")
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

        try:
            filename = jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini"
            # 参数范围：
            # xyz_interval  [0.1, 10]
            # rpy_interval  [0.1, 10]
            # vel  [0.001, 1800]
            # acc  [0.001, 3500]
            if (
                request.trajtrackconfig.xyz_interval < 0.1
                or request.trajtrackconfig.rpy_interval < 0.1
                or request.trajtrackconfig.vel < 0.001
                or request.trajtrackconfig.acc < 0.001
                or request.trajtrackconfig.xyz_interval > 10
                or request.trajtrackconfig.rpy_interval > 10
                or request.trajtrackconfig.vel > 1800
                or request.trajtrackconfig.acc > 3500
            ):
                grpc_logger().debug(
                    "Paramaters check [ FAILED ] \n {} {} {} {}".format(
                        request.trajtrackconfig.xyz_interval,
                        request.trajtrackconfig.rpy_interval,
                        request.trajtrackconfig.vel,
                        request.trajtrackconfig.acc,
                    )
                )
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )
            config = MyConfigParser()
            config.add_section("INTERVAL")
            config.set(
                "INTERVAL", "xyz_interval", str(request.trajtrackconfig.xyz_interval)
            )
            config.set(
                "INTERVAL", "rpy_interval", str(request.trajtrackconfig.rpy_interval)
            )

            config.add_section("ACC")
            config.set("ACC", "acc", str(request.trajtrackconfig.acc))

            config.add_section("VEL")
            config.set("VEL", "vel", str(request.trajtrackconfig.vel))

            with open(filename, "w+") as fd:
                config.write(fd)
            self.sync_path(filename)

            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTrajTrackConfig(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            track_config = jaka_pb2.TrajTrackConfig()
            filename = jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini"
            if os.path.exists(filename) and os.path.getsize(filename) > 10:
                config = MyConfigParser()
                config.read(jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini")

                track_config.xyz_interval = float(
                    config.get("INTERVAL", "xyz_interval")
                )
                track_config.rpy_interval = float(
                    config.get("INTERVAL", "rpy_interval")
                )
                track_config.acc = float(config.get("ACC", "acc"))
                track_config.vel = float(config.get("VEL", "vel"))
            else:
                config = MyConfigParser()
                config.add_section("INTERVAL")
                config.set("INTERVAL", "xyz_interval", str(0.5))
                config.set("INTERVAL", "rpy_interval", str(0.2))

                config.add_section("ACC")
                config.set("ACC", "acc", str(1000))

                config.add_section("VEL")
                config.set("VEL", "vel", str(300))

                with open(filename, "w+") as fd:
                    config.write(fd)
                    self.sync_path(filename)
                track_config.xyz_interval = float(0.5)
                track_config.rpy_interval = float(0.2)
                track_config.acc = float(1000)
                track_config.vel = float(300)

            return jaka_pb2.Response(errcode=0, trajtrackconfig=track_config)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetSubTrajTrackConfig(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if len(str(request.subtrajtrackconfig.dirname)) == 0:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )
            # filename = os.path.join(jkzuc.jkroot_dir + "/scripts/track/",str(request.subtrajtrackconfig.dirname),"/subtrajconfig.ini")
            filename = (
                jkzuc.jkroot_dir
                + "/scripts/track/"
                + str(request.subtrajtrackconfig.dirname)
                + "/subtrajconfig.ini"
            )
            grpc_logger().debug(filename)
            config = MyConfigParser()
            config.add_section("ACC")
            config.set("ACC", "acc", str(request.subtrajtrackconfig.acc))

            config.add_section("VEL")
            config.set("VEL", "vel", str(request.subtrajtrackconfig.vel))

            with open(filename, "w+") as fd:
                config.write(fd)
            self.sync_path(filename)

            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetSubTrajTrackConfig(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            track_config = jaka_pb2.SubTrajTrackConfig()
            # filename = os.path.join(jkzuc.jkroot_dir + "/scripts/track/", str(request.subtrajtrackconfig.dirname), "/subtrajconfig.ini")
            filename = (
                jkzuc.jkroot_dir
                + "/scripts/track/"
                + str(request.subtrajtrackconfig.dirname)
                + "/subtrajconfig.ini"
            )
            grpc_logger().debug(filename)
            if os.path.exists(filename):
                config = MyConfigParser()
                config.read(filename)

                track_config.acc = float(config.get("ACC", "acc"))
                track_config.vel = float(config.get("VEL", "vel"))
            else:
                config = MyConfigParser()
                config.add_section("INTERVAL")

                config.add_section("ACC")
                config.set("ACC", "acc", str(1000))

                config.add_section("VEL")
                config.set("VEL", "vel", str(300))

                with open(filename, "w+") as fd:
                    config.write(fd)
                self.sync_path(filename)
                track_config.acc = float(1000)
                track_config.vel = float(300)

            return jaka_pb2.Response(errcode=0, subtrajtrackconfig=track_config)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GenarateNgcScript(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            file_path = str(request.trajrawdatapath)
            if len(file_path) > 128 or len(file_path) == 0:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )
            track_config = jaka_pb2.TrajTrackConfig()

            config = MyConfigParser()
            config.read(jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini")
            track_config.xyz_interval = float(config.get("INTERVAL", "xyz_interval"))
            track_config.rpy_interval = float(config.get("INTERVAL", "rpy_interval"))
            track_config.acc = float(config.get("ACC", "acc"))
            track_config.vel = float(config.get("VEL", "vel"))

            com = [
                track_config.xyz_interval,
                track_config.rpy_interval,
                track_config.vel,
                track_config.acc,
            ]

            res = c.generate_ngc_file(str(file_path), com)
            if not res[0]:
                return jaka_pb2.Response(
                    errcode=int(jaka_pb2.RPC_ERR_GENE_SCRIPT),
                    errormsg="Exception:failed to generate scripts for trejectory reproduction",
                )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def TrajSample(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        from stat import S_IWRITE

        try:
            if int(c.get_traj_sample_status()) == int(request.trajsample):
                return jaka_pb2.Response(errcode=0)
            grpc_logger().debug(c.get_traj_sample_status())
            # 查看默认全局配置参数是否存在，不存在就创建
            if not os.path.exists(jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini"):
                filename = jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini"

                config = MyConfigParser()
                config.add_section("INTERVAL")
                config.set("INTERVAL", "xyz_interval", str("0.1"))
                config.set("INTERVAL", "rpy_interval", str("0.1"))

                config.add_section("ACC")
                config.set("ACC", "acc", str("1000"))

                config.add_section("VEL")
                config.set("VEL", "vel", str("300"))

                with open(filename, "w+") as fd:
                    config.write(fd)
                self.sync_path(filename)

            dirpath = jkzuc.jkroot_dir + "/scripts/track"
            if not os.path.exists(dirpath):
                os.mkdir(dirpath)
                time.sleep(0.5)
            if not os.access(dirpath, os.W_OK):
                os.chmod(dirpath, S_IWRITE)
                time.sleep(0.5)

            if not c.get_traj_sample_status():
                if not request.trajsample:
                    return jaka_pb2.Response(errcode=0)

            res = c.traj_sample(int(request.trajsample))
            if int(request.trajsample):
                if not res[0]:
                    return jaka_pb2.Response(
                        errcode=1, errormsg="Exception:grpc call failed"
                    )

            if not int(request.trajsample):
                track_config = jaka_pb2.TrajTrackConfig()

                config = MyConfigParser()
                config.read(jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini")
                track_config.xyz_interval = float(
                    config.get("INTERVAL", "xyz_interval")
                )
                track_config.rpy_interval = float(
                    config.get("INTERVAL", "rpy_interval")
                )
                track_config.acc = float(config.get("ACC", "acc"))
                track_config.vel = float(config.get("VEL", "vel"))
                com = [
                    track_config.xyz_interval,
                    track_config.rpy_interval,
                    track_config.vel,
                    track_config.acc,
                ]
                time.sleep(1.5)
                res = c.generate_ngc_file(str(res[0]), com)
                if not res[0]:
                    return jaka_pb2.Response(errcode=28, errormsg="generate ngc failed")

            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def IsInSamplingStatus(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            res = c.get_traj_sample_status()
            return jaka_pb2.Response(errcode=0, isintrajsampling=res[0])
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def RenameDirAndFile(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        update_jkzuc_status()
        # 程序运行时不允许修改轨迹文件名
        if s.interp_state != jkzuc.INTERP_IDLE:
            grpc_logger().debug("Program is running, can not modify script file name")
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")
        try:
            dirname = request.renametrackdirfile.src

            # 检查目标文件的长度
            if (
                len(request.renametrackdirfile.dest) > 128
            ):  # 若修改len ，需同步修改 call_movs_lib() 中的 path_buff_size
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_VAL_OUT_OF_RANGE,
                    errormsg="dest: {} is too long".format(
                        request.renametrackdirfile.dest
                    ),
                )
            if len(request.renametrackdirfile.dest) == 0:
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_VAL_OUT_OF_RANGE,
                    errormsg="RenameDirAndFile dest is null",
                )

            l = [jkzuc.jkroot_dir + "/scripts/track/", dirname]
            path = "".join(l)
            for file in os.listdir(path):
                if (
                    os.path.isfile(os.path.join(path, file)) == True
                    and file != "subtrajconfig.ini"
                ):
                    # 获取文件路径和扩展名
                    file_path = os.path.join(path, file)
                    base_name = os.path.basename(file_path)
                    file_name, extension = os.path.splitext(base_name)
                    newname = request.renametrackdirfile.dest + extension
                    os.rename(file_path, os.path.join(path, newname))
            path = jkzuc.jkroot_dir + "/scripts/track/"
            for file in os.listdir(path):
                if file == request.renametrackdirfile.src:
                    newname = file.replace(
                        request.renametrackdirfile.src, request.renametrackdirfile.dest
                    )
                    os.rename(os.path.join(path, file), os.path.join(path, newname))
            # c.traj_sample_begin()
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetExecutingScriptState(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        try:
            resp = jaka_pb2.Response(errcode=0)
            file_execute_state = resp.scriptExeState
            s.poll()
            for call in s.task_call_stack:
                if len(call["file"]) == 0:
                    break
                filestate = file_execute_state.state.add()
                filestate.filename = call["file"].replace(
                    jkzuc.jkroot_dir + "/scripts/", ""
                )
                rs = s.step_status
                if int(rs[2]) == 1:
                    filestate.currentline = -1
                else:
                    filestate.currentline = call["line"]
            return resp
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetScriptThreadState(self, request, context):
        try:
            resp = jaka_pb2.Response(errcode=0)
            thread_state = resp.scriptThreadState
            update_jkzuc_status()
            lst = s.sub_thread_state
            for i in range(5):
                sts = thread_state.state.add()
                sts.currentline = 0
                if lst[i] == "":
                    sts.filename = ""
                else:
                    sts.filename = lst[i].replace('"', "")

            return resp
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetDISafetyFunc(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        update_jkzuc_status()
        if s.cab_type != 2:
            return jaka_pb2.Response(errcode=0)

        try:
            currFuncDIMaps = list(s.funcdi)
            currSafetyFuncDISettings = list(s.safety_funcdi)
            for sDIFuncMap in request.sDIFuncSettings.settings:
                if (
                    sDIFuncMap.chnId < 0
                    or sDIFuncMap.chnId >= 8
                    or sDIFuncMap.action < jaka_pb2.SDI_ACTION_NONE
                    or sDIFuncMap.action > jaka_pb2.SDI_ACTION_COLLISION_LEVEL7
                ):
                    return jaka_pb2.Response(
                        errcode=1, errormsg="Exception: invalid parameters"
                    )
                for diFuncMap in currFuncDIMaps:
                    if (
                        sDIFuncMap.action > 1
                        and diFuncMap[0] == 0
                        and (
                            diFuncMap[1] == sDIFuncMap.chnId
                            or diFuncMap[1] == sDIFuncMap.chnId + 8
                        )
                    ):
                        return jaka_pb2.Response(
                            errcode=26, errormsg="Exception: pin is occupied"
                        )
                currSafetyFuncDISettings[sDIFuncMap.chnId] = sDIFuncMap.action
            c.set_safety_funcdi(currSafetyFuncDISettings)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetDISafetyFunc(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        update_jkzuc_status()
        sDIFuncMaps = jaka_pb2.SDIFuncSettings()
        if s.cab_type != 2:
            return jaka_pb2.Response(errcode=0, sDIFuncSettings=sDIFuncMaps)

        try:
            for chnId, action in enumerate(s.safety_funcdi):
                unitDIFuncSetting = sDIFuncMaps.settings.add()
                unitDIFuncSetting.chnId = chnId
                unitDIFuncSetting.action = action
            return jaka_pb2.Response(errcode=0, sDIFuncSettings=sDIFuncMaps)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetDOSafetyFunc(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        update_jkzuc_status()
        if s.cab_type != 2:
            return jaka_pb2.Response(errcode=0)

        try:
            currFuncDOMaps = list(s.funcdo)
            currSafetyFuncDOSettings = list(s.safety_funcdo)
            for sDOFuncMap in request.sDOFuncSettings.settings:
                if (
                    sDOFuncMap.chnId < 0
                    or sDOFuncMap.chnId >= 8
                    or sDOFuncMap.action < jaka_pb2.SDO_ACTION_NONE
                    or sDOFuncMap.action > jaka_pb2.SDO_ACTION_COLLISION_LEVEL7
                ):
                    return jaka_pb2.Response(
                        errcode=1, errormsg="Exception: invalid parameters"
                    )
                # 检查是否有功能DO与配置项冲突
                for doFuncMap in currFuncDOMaps:
                    for pinFuncMap in doFuncMap:
                        if (
                            sDOFuncMap.action > 1
                            and pinFuncMap[0] == 0
                            and (
                                pinFuncMap[1] == sDOFuncMap.chnId
                                or pinFuncMap[1] == sDOFuncMap.chnId + 8
                            )
                        ):
                            return jaka_pb2.Response(
                                errcode=26, errormsg="Exception: pin is occupied"
                            )
                currSafetyFuncDOSettings[sDOFuncMap.chnId] = sDOFuncMap.action
            c.set_safety_funcdo(currSafetyFuncDOSettings)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetDOSafetyFunc(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        update_jkzuc_status()
        sDOFuncMaps = jaka_pb2.SDOFuncSettings()
        if s.cab_type != 2:
            return jaka_pb2.Response(errcode=0, sDOFuncSettings=sDOFuncMaps)

        try:
            for chnId, action in enumerate(s.safety_funcdo):
                unitDOFuncSetting = sDOFuncMaps.settings.add()
                unitDOFuncSetting.chnId = chnId
                unitDOFuncSetting.action = action
            return jaka_pb2.Response(errcode=0, sDOFuncSettings=sDOFuncMaps)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetAutoOp(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_auto_op(
                request.autooption.auto_enable, request.autooption.auto_program
            )
            grpc_logger().debug(
                "[gRPC] set auto option: {} {}".format(
                    request.autooption.auto_enable, request.autooption.auto_program
                )
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetAutoOp(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            auto_option = jaka_pb2.AutoOption()
            auto_option.auto_enable = s.auto_status[0]
            auto_option.auto_program = s.auto_status[1]
            return jaka_pb2.Response(errcode=0, autooption=auto_option)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTIOPinMode(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            for pinModeSet in request.tioPinCfg.tioPinMode:
                if pinModeSet.pinType >= 0 and pinModeSet.pinType <= 2:
                    if (
                        (
                            (pinModeSet.pinType == 1 and s.tio_pin_cfg[1] == 0xFF)
                            or (pinModeSet.pinType == 2 and s.tio_pin_cfg[2] == 1)
                        )
                        and s.torqsensor[0][0] == 1
                        and s.torqsensor[1][0] == 1
                    ):
                        grpc_logger().debug(
                            "the sensor base on this channel is now running {}".format(
                                int(jaka_pb2.RPC_TIO_TORQSNSOR_CHN_USED)
                            )
                        )
                        return jaka_pb2.Response(
                            errcode=int(jaka_pb2.RPC_TIO_TORQSNSOR_CHN_USED),
                            errormsg="the sensor base on this channel is now running",
                        )
                    c.tio_set_pin_mode(pinModeSet.pinType, pinModeSet.pinMode)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTIOPinMode(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            varTioPinCfg = jaka_pb2.TIOPinCfg()
            for i in range(0, 3):
                pinModeSet = varTioPinCfg.tioPinMode.add()
                pinModeSet.pinType = i
                pinModeSet.pinMode = s.tio_pin_cfg[i]
            return jaka_pb2.Response(errcode=0, tioPinCfg=varTioPinCfg)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTIOVoutParam(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            for voutSet in request.tioVoutCfg.tioVoutParam:
                if voutSet.paramType == 0:
                    c.tio_set_vout_ena(voutSet.paramValue)
                elif voutSet.paramType == 1:
                    c.tio_set_vout_vol(voutSet.paramValue)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTIOVoutParam(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            varTioVoutCfg = jaka_pb2.TIOVoutCfg()
            voutEnaSet = varTioVoutCfg.tioVoutParam.add()
            voutEnaSet.paramType = 0
            voutEnaSet.paramValue = s.tio_vout_cfg[0]
            voutVolSet = varTioVoutCfg.tioVoutParam.add()
            voutVolSet.paramType = 1
            voutVolSet.paramValue = s.tio_vout_cfg[1]
            return jaka_pb2.Response(errcode=0, tioVoutCfg=varTioVoutCfg)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTIORS485ChannelMode(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            for rschnModeCfg in request.tioRsChnModeCfg.tioRsChnMode:
                if rschnModeCfg.chnId < 0 or rschnModeCfg.chnId > 1:
                    return jaka_pb2.Response(
                        errcode=int(jaka_pb2.RPC_INVALID_ARGS),
                        errormsg="Exception: invalid parameters",
                    )
                if rschnModeCfg.chnMode == 2:
                    if (rschnModeCfg.chnId == 0 and s.tio_rschn_cfg[1][0] == 2) or (
                        rschnModeCfg.chnId == 1 and s.tio_rschn_cfg[0][0] == 2
                    ):
                        grpc_logger().debug(
                            "only one tio RS 485 channel can be used torque sensor {}".format(
                                int(jaka_pb2.RPC_TIO_TORQSNSOR_CHN_EXCE)
                            )
                        )
                        return jaka_pb2.Response(
                            errcode=int(jaka_pb2.RPC_TIO_TORQSNSOR_CHN_EXCE),
                            errormsg="only one tio RS 485 channel can be used torque sensor",
                        )
                else:
                    # TIO RS channel is used as torque sensor, and it's now occupied
                    if (
                        s.tio_rschn_cfg[rschnModeCfg.chnId][0] == 2
                        and s.torqsensor[0][0] == 1
                        and s.torqsensor[1][0] == 1
                    ):
                        grpc_logger().debug(
                            "the sensor base on this channel is now running {}".format(
                                int(jaka_pb2.RPC_TIO_TORQSNSOR_CHN_USED)
                            )
                        )
                        return jaka_pb2.Response(
                            errcode=int(jaka_pb2.RPC_TIO_TORQSNSOR_CHN_USED),
                            errormsg="the sensor base on this channel is now running",
                        )
                grpc_logger().debug(
                    "{} {}".format(rschnModeCfg.chnId, rschnModeCfg.chnMode)
                )
                c.tio_set_rs485_chn_mode(rschnModeCfg.chnId, rschnModeCfg.chnMode)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTIORS485ChannelMode(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            varTioRsChnModeCfg = jaka_pb2.TIORSChnModeCfg()
            for i in range(0, 2):
                rsChnModeCfg = varTioRsChnModeCfg.tioRsChnMode.add()
                rsChnModeCfg.chnId = i
                rsChnModeCfg.chnMode = s.tio_rschn_cfg[i][0]
            return jaka_pb2.Response(errcode=0, tioRsChnModeCfg=varTioRsChnModeCfg)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetTIORS485ChannelComm(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        # request.intid, request.mbSlaveSettings
        try:
            update_jkzuc_status()
            if request.intid < 0 or request.intid > 1:
                return jaka_pb2.Response(
                    errcode=int(jaka_pb2.RPC_INVALID_ARGS),
                    errormsg="Exception: invalid parameters",
                )
            if (
                s.tio_rschn_cfg[request.intid][0] == 2
                and s.torqsensor[0][0] == 1
                and s.torqsensor[1][0] == 1
            ):
                grpc_logger().debug(
                    "the sensor base on this channel is now running {}".format(
                        int(jaka_pb2.RPC_TIO_TORQSNSOR_CHN_USED)
                    )
                )
                return jaka_pb2.Response(
                    errcode=int(jaka_pb2.RPC_TIO_TORQSNSOR_CHN_USED),
                    errormsg="Exception: torque sensor based on this channel is working now",
                )
            commInfo = (
                request.mbSlaveSettings.rtuSlaveSetting.slaveId,
                request.mbSlaveSettings.rtuSlaveSetting.baudrate,
                request.mbSlaveSettings.rtuSlaveSetting.databit,
                request.mbSlaveSettings.rtuSlaveSetting.stopbit,
                request.mbSlaveSettings.rtuSlaveSetting.parity,
            )
            c.tio_set_rs485_chn_comm(request.intid, commInfo)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetTIORS485ChannelComm(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        # [IN] request.intid, [OUT] request.mbSlaveSettings
        try:
            update_jkzuc_status()
            if request.intid < 0 or request.intid > 1:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception: invalid parameters"
                )
            tioRSRtuComm = jaka_pb2.MBSlaveSetting()
            tioRSRtuComm.type = 0
            rtuSetting = s.tio_rschn_cfg[request.intid][1]
            tioRSRtuComm.rtuSlaveSetting.slaveId = rtuSetting[0]
            tioRSRtuComm.rtuSlaveSetting.baudrate = rtuSetting[1]
            tioRSRtuComm.rtuSlaveSetting.databit = rtuSetting[2]
            tioRSRtuComm.rtuSlaveSetting.stopbit = rtuSetting[3]
            tioRSRtuComm.rtuSlaveSetting.parity = rtuSetting[4]
            return jaka_pb2.Response(errcode=0, mbSlaveSettings=tioRSRtuComm)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def TIORS485AddSignal(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            for sig in request.tioMbRtuSigCfg.mbRtuSig:
                c.tio_add_rs_signal(sig.strId, sig.chnId, sig.sigType, sig.sigAddr)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def TIORS485DeleteSignal(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.tio_del_rs_signal(request.str)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def TIORS485UpdateSignal(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            ret = c.tio_update_rs_signal(request.str, request.flo)
            if ret[0]:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def TIORS485GetSignalList(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            varTioRSSigCfg = jaka_pb2.TIORSSigCfg()
            for i in range(0, len(s.tio_rs_signals)):
                if s.tio_rs_signals[i][0] != "":
                    tioRSSig = varTioRSSigCfg.mbRtuSig.add()
                    tioRSSig.strId = s.tio_rs_signals[i][0]
                    tioRSSig.chnId = s.tio_rs_signals[i][1]
                    tioRSSig.sigType = s.tio_rs_signals[i][2]
                    tioRSSig.sigAddr = s.tio_rs_signals[i][3]
                    tioRSSig.value = s.tio_rs_signals[i][4]
                    tioRSSig.frequency = s.tio_rs_signals[i][5]
            return jaka_pb2.Response(errcode=0, tioMbRtuSigCfg=varTioRSSigCfg)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def TIORS485SendCommand(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.intid < 0 or request.intid > 1:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception: invalid parameters"
                )
            cmdBuf = []
            for by in request.rsInstCmd.byBuffer:
                cmdBuf.append(ord(by))
            grpc_logger().debug("cmdBuf: {}".format(cmdBuf))
            c.tio_send_rs_command(request.intid, cmdBuf)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def DiagnosisControl(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.intid == 1:
                c.diag_collect_data(1)
                grpc_logger().debug("[gRPC] start to collect diagnosis data")
                return jaka_pb2.Response(errcode=0)
            elif request.intid == 0:
                c.diag_collect_data(0)
                grpc_logger().debug("[gRPC] stop collecting diagnosis data")
                return jaka_pb2.Response(errcode=0)
            else:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception: invalid parameters"
                )
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    @sim_mode_required(0)
    def ServoDebugEnable(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.intid == 1:
                self.servoDebugMode = 1
                c.servo_upgrade()
                os.system("halcmd unload IOAIO")
                with open(
                    jkzuc.jkroot_dir + "/scripts/servoUpgrade/appRun.txt", "w+"
                ) as output:
                    subprocess.Popen(
                        [
                            jkzuc.jkroot_dir + "/scripts/servoUpgrade/APP/AppRun",
                            "DEBUG",
                        ],
                        stdout=output,
                    )
                time.sleep(1)
                t = threading.Thread(target=self.servo_upgrade_monitor)
                t.daemon = True
                t.start()
                grpc_logger().debug("[gRPC] enter servo debug mode response == 0")
                return jaka_pb2.Response(errcode=0)
            elif request.intid == 0 and self.servoDebugMode == 1:
                grpc_logger().debug("servo debug exit")
                timer_reboot()
                self.servoDebugMode = 0
                grpc_logger().debug("[gRPC] exit servo debug mode and reboot")
                return jaka_pb2.Response(errcode=0)
            else:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception: invalid parameters"
                )
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetRobotSafetySettings(self, request, context):
        def limit_number(num, min_value, max_value):
            return min(max(num, min_value), max_value)

        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if (
                int(request.robotSafetySettings.custome) < -1
                or int(request.robotSafetySettings.custome) > 4
            ):
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_INVALID_ARGS, errormsg="invalid arguments"
                )

            robotSafety = jaka_pb2.RobotSafetySettings()
            robotSafety = request.robotSafetySettings
            custom = int(request.robotSafetySettings.custome)
            paramType = int(request.robotSafetySettings.paramType)
            momentumLimit = float(request.robotSafetySettings.momentumLimit)
            tcpVelLimit = float(request.robotSafetySettings.tcpVelLimit)
            robotPowerLimit = float(request.robotSafetySettings.robotPowerLimit)
            if custom == -1:  # 恢复原有自定义设置值， rs[1] 为原有自定义值
                update_jkzuc_status()
                rs = s.robot_safety
                robotSafety.momentumLimit = float(rs[1]["momentumLimit"])
                robotSafety.tcpVelLimit = float(rs[1]["tcpVelLimit"])
                robotSafety.robotPowerLimit = float(rs[1]["robotPowerLimit"])
                robotSafety.stoppingDistance = float(rs[1]["stoppingDistance"])
                robotSafety.stoppingTime = float(rs[1]["stoppingTime"])
                robotSafety.custome = 0
                c.set_robot_safety_settings(
                    robotSafety.custome,
                    0,
                    robotSafety.momentumLimit,
                    robotSafety.tcpVelLimit,
                    robotSafety.robotPowerLimit,
                    robotSafety.stoppingDistance,
                    robotSafety.stoppingTime,
                )
                return jaka_pb2.Response(errcode=0, robotSafetySettings=robotSafety)

            if custom == 0:  # 自定义
                c.get_robot_safety_settings()
                time.sleep(0.1)
                update_jkzuc_status()
                rs = s.robot_safety
                if (
                    request.robotSafetySettings.stoppingDistance == 0
                    and request.robotSafetySettings.stoppingTime == 0
                ):
                    stoppingDistance = float(rs[1]["stoppingDistance"])
                    stoppingTime = float(rs[1]["stoppingTime"])
                else:
                    stoppingDistance = limit_number(
                        float(request.robotSafetySettings.stoppingDistance), 100, 4000
                    )
                    stoppingTime = limit_number(
                        float(request.robotSafetySettings.stoppingTime), 0.1, 5.0
                    )

                print(
                    "stop time: {}, stop dist: {}, request.robotSafetySettings.stoppingDistance: {}".format(
                        stoppingTime,
                        stoppingDistance,
                        request.robotSafetySettings.stoppingDistance,
                    )
                )
            else:  # 快速设置
                c.get_robot_safety_settings()
                time.sleep(0.1)
                update_jkzuc_status()
                rs = s.robot_safety
                momentumLimit = float(rs[1 + custom]["momentumLimit"])
                tcpVelLimit = float(rs[1 + custom]["tcpVelLimit"])
                robotPowerLimit = float(rs[1 + custom]["robotPowerLimit"])
                stoppingDistance = float(rs[1 + custom]["stoppingDistance"])
                stoppingTime = float(rs[1 + custom]["stoppingTime"])
            ret = c.set_robot_safety_settings(
                custom,
                paramType,
                momentumLimit,
                tcpVelLimit,
                robotPowerLimit,
                stoppingDistance,
                stoppingTime,
            )
            if ret == 0:  # 参数设置成功后才设置力限制
                print("[grpc] set_robot_safety_settings success")
                if custom == 1:
                    # c.set_clsn_sensi(0)
                    c.set_clsn_sensi(request.robot_index, 7)
                if custom == 2:
                    c.set_clsn_sensi(request.robot_index, 5)
                if custom == 3:
                    c.set_clsn_sensi(request.robot_index, 3)
                if custom == 4:
                    c.set_clsn_sensi(request.robot_index, 1)
                c.get_robot_safety_settings()
                time.sleep(0.1)
                update_jkzuc_status()
                rs = s.robot_safety
                robotSafety.custome = int(rs[0]["custome"])
                robotSafety.momentumLimit = float(rs[1 + custom]["momentumLimit"])
                robotSafety.tcpVelLimit = float(rs[1 + custom]["tcpVelLimit"])
                robotSafety.robotPowerLimit = float(rs[1 + custom]["robotPowerLimit"])
                robotSafety.stoppingDistance = float(rs[1 + custom]["stoppingDistance"])
                robotSafety.stoppingTime = float(rs[1 + custom]["stoppingTime"])
            else:  # 若失败输出原始值
                print("[grpc] errrr! set_robot_safety_settings failed")
                c.get_robot_safety_settings()
                time.sleep(0.1)
                update_jkzuc_status()
                rs = s.robot_safety
                robotSafety.custome = int(rs[0]["custome"])
                robotSafety.momentumLimit = float(rs[0]["momentumLimit"])
                robotSafety.tcpVelLimit = float(rs[0]["tcpVelLimit"])
                robotSafety.robotPowerLimit = float(rs[0]["robotPowerLimit"])
                robotSafety.stoppingDistance = float(rs[0]["stoppingDistance"])
                robotSafety.stoppingTime = float(rs[0]["stoppingTime"])

            return jaka_pb2.Response(errcode=0, robotSafetySettings=robotSafety)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetRobotSafetySettings(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.get_robot_safety_settings()
            time.sleep(0.15)
            update_jkzuc_status()

            rs = s.robot_safety
            robotSafety = jaka_pb2.RobotSafetySettings()
            robotSafety.custome = int(rs[0]["custome"])
            robotSafety.lastCustome = int(rs[0]["lastCustome"])
            robotSafety.paramType = 0
            robotSafety.momentumLimit = float(rs[0]["momentumLimit"])
            robotSafety.tcpVelLimit = float(rs[0]["tcpVelLimit"])
            robotSafety.robotPowerLimit = float(rs[0]["robotPowerLimit"])
            robotSafety.stoppingDistance = float(rs[0]["stoppingDistance"])
            robotSafety.stoppingTime = float(rs[0]["stoppingTime"])
            print(robotSafety)
            return jaka_pb2.Response(errcode=0, robotSafetySettings=robotSafety)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetDefaultRobotSafetySettings(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            custome = int(request.robotSafetySettings.custome)
            c.get_robot_safety_settings()
            time.sleep(0.1)
            update_jkzuc_status()
            rs = s.robotSafety
            robotSafety = jaka_pb2.RobotSafetySettings()
            robotSafety.custome = custome
            robotSafety.paramType = 0
            robotSafety.momentumLimit = float(rs[custome + 1]["momentumLimit"])
            robotSafety.tcpVelLimit = float(rs[custome + 1]["tcpVelLimit"])
            robotSafety.robotPowerLimit = float(rs[custome + 1]["robotPowerLimit"])
            return jaka_pb2.Response(errcode=0, robotSafetySettings=robotSafety)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetJointSafetySettings(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            robindex = request.robot_index
            for jts in request.jointSafetySettings.joint_safety_settings:
                jointNum = int(jts.jointNum)
                paramType = int(jts.paramType)
                posLimt = jts.posLimt
                jointVelLimit = float(jts.jointVelLimit)
                jointErrLimit = float(jts.jointErrLimit)
                if paramType == 0:
                    # SetJointLimit
                    update_jkzuc_status()
                    minJointPosLimits = s.robot_model_data["minJointPosLimits"]
                    maxJointPosLimits = s.robot_model_data["maxJointPosLimits"]

                    if not (
                        posLimt.positive_limit <= maxJointPosLimits[jointNum]
                        and posLimt.negative_limit >= minJointPosLimits[jointNum]
                        and posLimt.positive_limit > posLimt.negative_limit
                    ):

                        errMsg = "joint {:d} min_limit is too small or max_limit is too large or min_limit not less than max_limit".format(
                            jointNum
                        )
                        return jaka_pb2.Response(errcode=6, errormsg=errMsg)
                    c.robot_set_joint_min_limit(robindex, jointNum, posLimt.negative_limit)
                    c.robot_set_joint_max_limit(robindex, jointNum, posLimt.positive_limit)
                else:
                    if jointErrLimit < 0 or jointErrLimit > 100:
                        return jaka_pb2.Response(
                            errcode=-1,
                            errormsg="jointErrLimit should be within 0 and 1",
                        )
                    c.set_joint_safety_settings(
                        robindex, jointNum, paramType, jointVelLimit, jointErrLimit
                    )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def __get_safe_joint_config(self, robid:int, jointNum:int) -> jaka_pb2.JointSafetySettings.JointSafetySetting():
        joint = jaka_pb2.JointSafetySettings.JointSafetySetting()
        joint.jointNum = jointNum
        rs = s.robot(robid).safe_joint_config
        joint.jointVelLimit = float(rs[jointNum]["max_vel"])
        joint.jointErrLimit = float(rs[jointNum]["err_limit"])
        joint.posLimt.joint_id = jointNum
        joint.posLimt.positive_limit = float(rs[jointNum]["max_pos"])
        joint.posLimt.negative_limit = float(rs[jointNum]["min_pos"])
        return joint

    def __get_safe_joint_def_config(self, robid:int, jointNum:int) -> jaka_pb2.JointSafetySettings.JointSafetySetting():
        joint = jaka_pb2.JointSafetySettings.JointSafetySetting()
        joint.jointNum = jointNum
        rs = s.robot(robid).model_data
        joint.jointVelLimit = float(rs["jvel_max"][jointNum])
        joint.jointErrLimit = float(rs["jerr_limit"][jointNum])
        joint.posLimt.joint_id = jointNum
        joint.posLimt.positive_limit = float(rs["jpos_max"][jointNum])
        joint.posLimt.negative_limit = float(rs["jpos_min"][jointNum])
        return joint

    def GetJointSafetySettings(self, request, context):
        '''
        获取用户配置的的关节安全设置
        '''
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.get_joint_safety_settings()
            jointNum = int(request.intid)
            rob_index = request.robot_index
            resp = jaka_pb2.Response(errcode=0)
            joint_limit = resp.jointSafetySettings
            update_jkzuc_status()
            if jointNum == -1: # 获取所有关节的安全设置
                robot_joint_num = s.robot(rob_index).robot_joint_num
                for i in range(0, robot_joint_num):
                    joint = self.__get_safe_joint_config(rob_index, i)
                    joint_limit.joint_safety_settings.append(joint)
            else: # 获取指定关节的安全设置
                joint = self.__get_safe_joint_config(rob_index, jointNum)
                joint_limit.joint_safety_settings.append(joint)
            return resp

        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetDefaultJointSafetySettings(self, request, context):
        '''
        获取机型的默认关节安全设置
        '''
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.get_joint_safety_settings()
            jointNum = int(request.intid)
            rob_index = request.robot_index
            resp = jaka_pb2.Response(errcode=0)
            joint_limit = resp.jointSafetySettings
            update_jkzuc_status()
            if jointNum == -1:
                robot_joint_num = s.robot(rob_index).robot_joint_num
                for i in range(0, robot_joint_num):
                    joint = self.__get_safe_joint_def_config(rob_index, i)
                    joint_limit.joint_safety_settings.append(joint)
            else:
                joint = self.__get_safe_joint_def_config(rob_index, jointNum)
                joint_limit.joint_safety_settings.append(joint)
            return resp
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetCollisionOption(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            clsnOpt = s.collision_option
            if request.intid == 0:
                clsnOpt[0] = request.collisionOpt.clsnDetectMethod
                clsnOpt[1] = request.collisionOpt.clsnReactTypeWhenProgRun
                clsnOpt[2] = request.collisionOpt.clsnReactReboundAngle
            elif request.intid == 1:
                clsnOpt[0] = request.collisionOpt.clsnDetectMethod
            elif request.intid == 2:
                clsnOpt[1] = request.collisionOpt.clsnReactTypeWhenProgRun
            elif request.intid == 3:
                clsnOpt[2] = request.collisionOpt.clsnReactReboundAngle
            if clsnOpt[2] < 0 or clsnOpt[2] > 3 or clsnOpt[1] > 1 or clsnOpt[1] < 0:
                return jaka_pb2.Response(
                    errcode=-1,
                    errormsg="Para Error! Reround angle should be within 0 and 3",
                )
            c.set_collision_option(clsnOpt[0], clsnOpt[1], clsnOpt[2])
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetCollisionOption(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            curClsnOpt = s.collision_option
            clsnOpt = jaka_pb2.CollisionOption()
            clsnOpt.clsnDetectMethod = curClsnOpt[0]
            clsnOpt.clsnReactTypeWhenProgRun = curClsnOpt[1]
            clsnOpt.clsnReactReboundAngle = curClsnOpt[2]
            return jaka_pb2.Response(errcode=0, collisionOpt=clsnOpt)

        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetScriptVariables(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            scriptVarDict = {}
            scriptVariables = jaka_pb2.ScriptVariables()
            if request.scriptVariableRequest.returnAllVar:
                grpc_logger().debug(request.scriptVariableRequest.returnAllVar)
                scriptVarDict = redis_conn.hgetall("scriptVariables")
                for k in scriptVarDict.keys():
                    if not scriptVarDict[k]:
                        del scriptVarDict[k]
                    if scriptVarDict[k].startswith("[") and scriptVarDict[k].endswith(
                        "]"
                    ):  # 识别数组变量
                        # 对数组变量进行格式化处理
                        list_value = eval(scriptVarDict[k])
                        updated_list = [
                            "{:.3f}".format(float(item)) for item in list_value
                        ]
                        scriptVarDict[k] = "[" + ", ".join(updated_list) + "]"
            else:
                scriptVarList = redis_conn.hmget(
                    "scriptVariables", request.scriptVariableRequest.varID
                )
                for i in range(len(request.scriptVariableRequest.varID)):
                    if scriptVarList[i] != None:
                        if scriptVarList[i].startswith("[") and scriptVarList[
                            i
                        ].endswith(
                            "]"
                        ):  # 识别数组变量
                            # 对数组变量进行格式化处理
                            list_value = eval(scriptVarList[i])
                            updated_list = [
                                "{:.3f}".format(float(item)) for item in list_value
                            ]
                            scriptVarList[i] = "[" + ", ".join(updated_list) + "]"
                        scriptVarDict[request.scriptVariableRequest.varID[i]] = (
                            scriptVarList[i]
                        )
            scriptVariables.varInfos.update(scriptVarDict)
            del scriptVarDict
            return jaka_pb2.Response(errcode=0, scriptVariables=scriptVariables)

        except redis.exceptions.ConnectionError:
            grpc_logger().debug("[gRPC] memory server is not running")
            return jaka_pb2.Response(
                errcode=32, errormsg="memory server is not running"
            )
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetCabinetID(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            cabid = s.monitor_data[6]
            type(cabid)
            if cabid >= 4294967295:
                cabid = 0
                grpc_logger().debug("cabid is {}.change to 0".format(cabid))
            return jaka_pb2.Response(errcode=0, intid=cabid)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def IsRobotInLimitedStatus(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        try:
            retStatus = jaka_pb2.RobotLimitedStatus()
            update_jkzuc_status()
            robotLimitedStaus = s.is_robot_in_limited_status
            retStatus.isReducedLevel1 = robotLimitedStaus[0]
            retStatus.isReducedLevel2 = robotLimitedStaus[1]
            retStatus.isProtectiveStop = robotLimitedStaus[2]
            retStatus.isMomentumLimited = robotLimitedStaus[3]
            retStatus.isTcpVelLimited = robotLimitedStaus[4]
            retStatus.isRobotPowerLimited = robotLimitedStaus[5]
            retStatus.isSafeZoneLimited = robotLimitedStaus[6]
            retStatus.isReducedMode = robotLimitedStaus[7]
            retStatus.isThreePositionEnableLimit = robotLimitedStaus[8]
            return jaka_pb2.Response(errcode=0, robotLimitedStatus=retStatus)

        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetProfinetConfig(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            pn_config = jkzuc.jkroot_dir + "/configs/JAKA/pn_config.bin"
            if request.profinetConfig.reset:
                if os.path.exists(pn_config):
                    os.remove(pn_config)
                    os.mknod(pn_config)
            c.config_profinet(request.profinetConfig.enable)
            if request.profinetConfig.enable == 0:
                c.clear_profinet_funcio()

            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetProfinetConfig(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            retStatus = jaka_pb2.ProfinetConfig()
            retStatus.reset = 0
            setting = jkzuc.jkroot_dir + "/configs/JAKA/usersettings.ini"
            config = configparser.ConfigParser()
            config.read(setting)
            en = config.get("PROFINET", "ENABLE")
            retStatus.enable = int(en)
            return jaka_pb2.Response(errcode=0, profinetConfig=retStatus)

        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetEIP(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            global GET_EIP_ENABLE
            EIP_ENABLE = request.eipConfig.eip_enable
            GET_EIP_ENABLE = EIP_ENABLE
            c.set_eip(EIP_ENABLE)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
            )

    def GetEIP(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            eipConfig = jaka_pb2.EIPConfig()
            setting = jkzuc.jkroot_dir + "/configs/JAKA/usersettings.ini"
            config = configparser.ConfigParser()
            config.read(setting)
            en = config.get("EIP", "ENABLE", fallback="0")
            eipConfig.eip_enable = int(en)
            global GET_EIP_ENABLE
            GET_EIP_ENABLE = int(en)
            filename = "/etc/systemd/network/primary.network"
            if int(en) == 1:
                config.read(filename)
                if config.has_option("Network", "DHCP"):
                    eipConfig.dhcporstaticip.type = 0
                else:
                    eipConfig.dhcporstaticip.type = 1
                    ipaddr = (config.get("Network", "Address").split("/"))[0]
                    prefix_length = (config.get("Network", "Address").split("/"))[1]
                    gateway = config.get("Network", "Gateway")
                    prefix_bin = "1" * int(prefix_length) + "0" * (
                        32 - int(prefix_length)
                    )
                    netmask_parts = []
                    for i in range(0, 32, 8):
                        netmask_part = prefix_bin[i : i + 8]
                        netmask_parts.append(str(int(netmask_part, 2)))
                    netmask = ".".join(netmask_parts)
                    eipConfig.dhcporstaticip.ip = ipaddr
                    eipConfig.dhcporstaticip.netmask = netmask
                    eipConfig.dhcporstaticip.gateway = gateway

            return jaka_pb2.Response(errcode=jaka_pb2.RPC_OK, eipConfig=eipConfig)

        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
            )

    def SetFreeButtonOpt(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_free_button_opt(request.intid)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetFreeButtonOpt(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            return jaka_pb2.Response(errcode=0, intid=s.free_button_opt[0])
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetFreeProgButtonOpt(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_free_prog_button_opt(
                request.freeProgButton.freeOpt, request.freeProgButton.progOpt
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetFreeProgButtonOpt(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            freeProgButton = jaka_pb2.TioFreeProgButton()
            update_jkzuc_status()
            option = s.free_prog_button_opt
            freeProgButton.freeOpt = option[0]
            freeProgButton.progOpt = option[1]
            return jaka_pb2.Response(errcode=0, freeProgButton=freeProgButton)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetPointButtonOpt(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_point_button_opt(request.intid)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetPointButtonOpt(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            return jaka_pb2.Response(errcode=0, intid=s.point_button_opt[0])
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetIOConfigName(self, ioType):
        global fileOpen
        fileOpen += 1
        filename = jkzuc.jkroot_dir + "/configs/JAKA/aliasname.ini"
        config = configparser.ConfigParser()
        config.read(filename)

        def GetConfigSecItems(secName):
            ret = []
            try:
                ret = config.items(secName)
            except configparser.NoSectionError:
                grpc_logger().error("Section cannot be found: {}".format(secName))
            return ret

        if ioType == 0 or ioType == -1:
            for key, value in GetConfigSecItems("STD_DI"):
                SET_NAME_STDIO_DI.update({key: value})
            for key, value in GetConfigSecItems("STD_DO"):
                SET_NAME_STDIO_DO.update({key: value})
            for key, value in GetConfigSecItems("STD_AI"):
                SET_NAME_STDIO_AI.update({key: value})
            for key, value in GetConfigSecItems("STD_AO"):
                SET_NAME_STDIO_AO.update({key: value})
        if ioType == 1 or ioType == -1:
            for key, value in GetConfigSecItems("TIO_DI"):
                SET_NAME_TIO_DI.update({key: value})
            for key, value in GetConfigSecItems("TIO_DO"):
                SET_NAME_TIO_DO.update({key: value})
            for key, value in GetConfigSecItems("TIO_AI"):
                SET_NAME_TIO_AI.update({key: value})
            for key, value in GetConfigSecItems("TIO_AO"):
                SET_NAME_TIO_AO.update({key: value})

        if ioType == 2 or ioType == -1:
            for key, value in GetConfigSecItems("EXT_DI"):
                SET_NAME_EXTIO_DI.update({key: value})
            for key, value in GetConfigSecItems("EXT_DO"):
                SET_NAME_EXTIO_DO.update({key: value})
            for key, value in GetConfigSecItems("EXT_AI"):
                SET_NAME_EXTIO_AI.update({key: value})
            for key, value in GetConfigSecItems("EXT_AO"):
                SET_NAME_EXTIO_AO.update({key: value})

        if ioType == 4 or ioType == -1:
            for key, value in GetConfigSecItems("MB_SLAVE_DI"):
                SET_NAME_MB_DI.update({key: value})
            for key, value in GetConfigSecItems("MB_SLAVE_DO"):
                SET_NAME_MB_DO.update({key: value})
            for key, value in GetConfigSecItems("MB_SLAVE_AI"):
                SET_NAME_MB_AI.update({key: value})
            for key, value in GetConfigSecItems("MB_SLAVE_AO"):
                SET_NAME_MB_AO.update({key: value})
        if ioType == 5 or ioType == -1:
            for key, value in GetConfigSecItems("PN_DEV_DI"):
                SET_NAME_PFN_DI.update({key: value})
            for key, value in GetConfigSecItems("PN_DEV_DO"):
                SET_NAME_PFN_DO.update({key: value})
            for key, value in GetConfigSecItems("PN_DEV_AI"):
                SET_NAME_PFN_AI.update({key: value})
            for key, value in GetConfigSecItems("PN_DEV_AO"):
                SET_NAME_PFN_AO.update({key: value})
        if ioType == 6 or ioType == -1:
            for key, value in GetConfigSecItems("EIP_ADPT_DI"):
                SET_NAME_EIP_DI.update({key: value})
            for key, value in GetConfigSecItems("EIP_ADPT_DO"):
                SET_NAME_EIP_DO.update({key: value})
            for key, value in GetConfigSecItems("EIP_ADPT_AI"):
                SET_NAME_EIP_AI.update({key: value})
            for key, value in GetConfigSecItems("EIP_ADPT_AO"):
                SET_NAME_EIP_AO.update({key: value})

    def GetIOConfigInfo(self, request, context):
        """
        FIXME:这个函数太丑陋了，后续需要重构
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            global SET_NAME_STDIO_DI
            global SET_NAME_STDIO_DO
            global SET_NAME_STDIO_AI
            global SET_NAME_STDIO_AO

            global SET_NAME_TIO_DI
            global SET_NAME_TIO_DO
            global SET_NAME_TIO_AI
            global SET_NAME_TIO_AO

            global SET_NAME_EXTIO_DI
            global SET_NAME_EXTIO_DO
            global SET_NAME_EXTIO_AI
            global SET_NAME_EXTIO_AO
            SET_NAME_EXTIO_DI = {}
            SET_NAME_EXTIO_DO = {}
            SET_NAME_EXTIO_AI = {}
            SET_NAME_EXTIO_AO = {}

            global SET_NAME_MB_DI
            global SET_NAME_MB_DO
            global SET_NAME_MB_AI
            global SET_NAME_MB_AO

            global SET_NAME_PFN_DI
            global SET_NAME_PFN_DO
            global SET_NAME_PFN_AI
            global SET_NAME_PFN_AO

            global SET_NAME_EIP_DI
            global SET_NAME_EIP_DO
            global SET_NAME_EIP_AI
            global SET_NAME_EIP_AO

            ioConfigInfo = jaka_pb2.IOConfigInfo()
            ioType = int(request.ioConfigInfo.ioType)
            # ioType = 2
            # c.set_io_type(ioType)
            # ioNum = s.get_io_num
            grpc_logger().debug("ioType: {}".format(ioType))
            if fileOpen == 1 or ioType == 2 or ioType == -1:
                self.GetIOConfigName(ioType)
            update_jkzuc_status()
            global cabMaxDIO
            cabMaxAI = 0
            cabMaxAO = 0
            global modMaxDIO
            modMaxAIO = 0
            currFuncDIMap = s.funcdi
            currFuncDOMap = s.funcdo
            cabType = s.cab_type
            grpc_logger().debug("cabTpe: {}".format(cabType))
            if cabType == 2:
                scbAnaChnSettings = s.scb_anachn_setting
                # grpc_logger().debug("scbAnaChnSetting {}".format(scbAnaChnSettings))

            if cabType == 3:
                scbDigChnSettings = s.scb_digchn_setting
                # grpc_logger().debug("scbDigChnSettings {}".format(scbDigChnSettings))

            if ioType == 0 or ioType == -1:  # Cabinet io
                grpc_logger().debug("Cabinet io")
                # c.set_io_type(0)
                ioNum = s.get_io_num(0)
                # io number

                ioConfigInfo.cabIOConfigInfo.diNum = ioNum[0]
                cabMaxDIO = ioNum[0]
                ioConfigInfo.cabIOConfigInfo.doNum = ioNum[1]
                ioConfigInfo.cabIOConfigInfo.aiNum = ioNum[2]
                cabMaxAI = ioNum[2]
                ioConfigInfo.cabIOConfigInfo.aoNum = ioNum[3]
                cabMaxAO = ioNum[3]
                # index name
                for i in range(ioNum[0]):
                    diInfo = ioConfigInfo.cabIOConfigInfo.diIf.add()
                    diInfo.index = i

                    if str(i) in SET_NAME_STDIO_DI:
                        diInfo.name = SET_NAME_STDIO_DI[str(i)]
                    else:
                        if cabType == 1:
                            diInfo.name = "DI_" + str(i)
                        else:
                            diInfo.name = "DI_" + str(i + 1)

                    if cabType == 3:
                        # reused:   0: input,  1: output
                        diInfo.reusedInfo = (scbDigChnSettings[i] + 1) % 2

                    if cabType == 1 and i == 0:
                        diInfo.reusedInfo = 2
                    else:
                        for idx in range(0, FUNCDI_NUM):
                            if (
                                currFuncDIMap[idx][0] == 0
                                and currFuncDIMap[idx][1] == i
                            ):
                                diInfo.diFuncIO = idx + 1
                                break

                # index name
                for i in range(ioNum[1]):
                    doInfo = ioConfigInfo.cabIOConfigInfo.doIf.add()
                    doInfo.index = i
                    if str(i) in SET_NAME_STDIO_DO:
                        doInfo.name = SET_NAME_STDIO_DO[str(i)]
                    else:
                        if cabType == 1:
                            doInfo.name = "DO_" + str(i)
                        else:
                            doInfo.name = "DO_" + str(i + 1)

                    if cabType == 3:
                        # reused: 0: input,  1: output
                        doInfo.reusedInfo = (scbDigChnSettings[i] + 1) % 2

                    if cabType == 1 and i == 0:
                        doInfo.reusedInfo = 2
                    else:
                        flag = 0
                        for idx in range(0, FUNCDO_NUM):
                            for idy in range(0, 4):
                                if (
                                    currFuncDOMap[idx][idy][0] == 0
                                    and currFuncDOMap[idx][idy][1] == i
                                ):
                                    doInfo.doFuncIO = idx + 1
                                    flag = 1
                                    break
                            if flag == 1:
                                flag = 0
                                break

                #index name
                for i in range(ioNum[2]):
                    aiInfo = ioConfigInfo.cabIOConfigInfo.aiIf.add()
                    aiInfo.index = i
                    if str(i) in SET_NAME_STDIO_AI:
                        aiInfo.name = SET_NAME_STDIO_AI[str(i)]
                    else:
                        aiInfo.name = "AI_" + str(i + 1)
                    if cabType == 2:
                        if scbAnaChnSettings[i] == 0:
                            aiInfo.reusedInfo = 88  # notReused = 88; not reused io
                        elif scbAnaChnSettings[i] == 1:
                            aiInfo.reusedInfo = 3
                        elif scbAnaChnSettings[i] == 2:
                            aiInfo.reusedInfo = 4
                        elif scbAnaChnSettings[i] == 3:
                            aiInfo.reusedInfo = 88
                        elif scbAnaChnSettings[i] == 4:
                            aiInfo.reusedInfo = 88

                # index name
                for i in range(ioNum[3]):
                    aoInfo = ioConfigInfo.cabIOConfigInfo.aoIf.add()
                    aoInfo.index = i
                    if str(i) in SET_NAME_STDIO_AO:
                        aoInfo.name = SET_NAME_STDIO_AO[str(i)]
                    else:
                        aoInfo.name = "AO_" + str(i + 1)
                    if cabType == 2:
                        if scbAnaChnSettings[i] == 0:
                            aoInfo.reusedInfo = 88  # notReused = 8; not reused io
                        elif scbAnaChnSettings[i] == 1:
                            aoInfo.reusedInfo = 88
                        elif scbAnaChnSettings[i] == 2:
                            aoInfo.reusedInfo = 88
                        elif scbAnaChnSettings[i] == 3:
                            aoInfo.reusedInfo = 5
                        elif scbAnaChnSettings[i] == 4:
                            aoInfo.reusedInfo = 6

            if ioType == 1 or ioType == -1:  # tool io
                grpc_logger().debug("Tool io")
                # c.set_io_type(1)
                ioNum = s.get_io_num(1)
                tio_hardware_ver = tool_function.getTioVer(s.tio_version)[2]
                ioConfigInfo.tioInfo.version = s.tio_version
                ioConfigInfo.tioInfo.diNum = ioNum[0]
                ioConfigInfo.tioInfo.doNum = ioNum[1]
                ioConfigInfo.tioInfo.aiNum = ioNum[2]
                if tio_hardware_ver == 3:
                    diPinMode = s.tio_pin_cfg[0]
                    doPinMode = s.tio_pin_cfg[1]
                    aiPinMode = s.tio_pin_cfg[2]

                # index name
                for i in range(ioNum[0]):
                    diInfo = ioConfigInfo.tioInfo.diIf.add()
                    diInfo.index = i
                    if str(i) in SET_NAME_TIO_DI:
                        diInfo.name = SET_NAME_TIO_DI[str(i)]
                    else:
                        diInfo.name = "DI_" + str(i + 1)
                    for idx in range(0, FUNCDI_NUM):
                        if currFuncDIMap[idx][0] == 1 and currFuncDIMap[idx][1] == i:
                            diInfo.diFuncIO = idx + 1
                            break
                    if tio_hardware_ver == 3:
                        if 0x0F & diPinMode == 0:
                            diInfo.reusedInfo = 7
                        else:
                            diInfo.reusedInfo = 8
                        diPinMode = diPinMode >> 4

                # index name
                for i in range(ioNum[1]):
                    doInfo = ioConfigInfo.tioInfo.doIf.add()
                    doInfo.index = i
                    if str(i) in SET_NAME_TIO_DO:
                        doInfo.name = SET_NAME_TIO_DO[str(i)]
                    else:
                        doInfo.name = "DO_" + str(i + 1)
                    flag = 0
                    for idx in range(0, FUNCDO_NUM):
                        for idy in range(0, 4):
                            if (
                                currFuncDOMap[idx][idy][0] == 1
                                and currFuncDOMap[idx][idy][1] == i
                            ):
                                doInfo.doFuncIO = idx + 1
                                flag = 1
                                break
                        if flag == 1:
                            flag = 0
                            break

                    if tio_hardware_ver == 3:
                        if 0x0F & doPinMode == 0x00:
                            doInfo.reusedInfo = 7
                        elif 0x0F & doPinMode == 0x01:
                            doInfo.reusedInfo = 8
                        elif 0x0F & doPinMode == 0x02:
                            doInfo.reusedInfo = 9
                        elif 0x0F & doPinMode == 0x0F:
                            doInfo.reusedInfo = 10
                        doPinMode = doPinMode >> 4

                # index name
                for i in range(ioNum[2]):
                    aiInfo = ioConfigInfo.tioInfo.aiIf.add()
                    aiInfo.index = i
                    if str(i) in SET_NAME_TIO_AI:
                        aiInfo.name = SET_NAME_TIO_AI[str(i)]
                    else:
                        aiInfo.name = "AI_" + str(i + 1)
                    if tio_hardware_ver == 3:
                        if 0x0F & aiPinMode == 0:
                            aiInfo.reusedInfo = 1
                        else:
                            aiInfo.reusedInfo = 10

            if ioType == 2 or ioType == -1:  # extio
                grpc_logger().debug("extio")
                # c.set_io_type(2)
                # ioNum = s.get_io_num
                CURRENT_EXTIO_DI_NUM = 0
                CURRENT_EXTIO_DO_NUM = 0
                CURRENT_EXTIO_AI_NUM = 0
                CURRENT_EXTIO_AO_NUM = 0
                # io number
                for i in range(s.extio_num):
                    extio_setup = s.extio_setup[i]
                    extioModConfig = ioConfigInfo.extioconfig.add()
                    ioModSetup = extioModConfig.extIOModSetup
                    ioModSetup.type = extio_setup[0]
                    ioModSetup.modName = extio_setup[1]
                    if ioModSetup.type == 0:  # MOD-RTU
                        ioModSetup.rtuComm.baudrate = extio_setup[2][0]
                        ioModSetup.rtuComm.bits = extio_setup[2][1]
                        ioModSetup.rtuComm.datalength = extio_setup[2][1]
                        ioModSetup.rtuComm.slaveId = extio_setup[2][2]
                        ioModSetup.rtuComm.stopbit = extio_setup[2][4]
                        ioModSetup.rtuComm.parity = extio_setup[2][3]
                        if (
                            request.extiosetup.rtuComm.slaveId > 247
                            or request.extiosetup.rtuComm.slaveId < 0
                        ):
                            grpc_logger().debug(
                                "slaveId %d out of range [%d,%d]"
                                % (request.extiosetup.rtuComm.slaveId, 0, 247)
                            )
                            return jaka_pb2.Response(
                                errcode=15,
                                errormsg="Exception: parameter slaveId out of range",
                            )
                    else:
                        ioModSetup.tcpComm.ipaddr = extio_setup[2][0]
                        ioModSetup.tcpComm.port = extio_setup[2][1]
                        ioModSetup.tcpComm.slaveId = extio_setup[2][2]

                    extio_pin_setup = extio_setup[3]
                    ioModSetup.diSetup.startaddr = extio_pin_setup[0][0]
                    ioModSetup.diSetup.pinnum = extio_pin_setup[0][1]
                    ioModSetup.doSetup.startaddr = extio_pin_setup[1][0]
                    ioModSetup.doSetup.pinnum = extio_pin_setup[1][1]
                    ioModSetup.aiSetup.startaddr = extio_pin_setup[2][0]
                    ioModSetup.aiSetup.pinnum = extio_pin_setup[2][1]
                    ioModSetup.aoSetup.startaddr = extio_pin_setup[3][0]
                    ioModSetup.aoSetup.pinnum = extio_pin_setup[3][1]
                    ioModSetup.ExtIOModInfo = extio_setup[4]

                    ioModPinMap = extioModConfig.extIOModPinMap
                    extio_pinmap = s.extio_pinmap[i]
                    ioModPinMap.diStartIdx = extio_pinmap[0]
                    ioModPinMap.doStartIdx = extio_pinmap[1]
                    ioModPinMap.aiStartIdx = extio_pinmap[2]
                    ioModPinMap.aoStartIdx = extio_pinmap[3]

                    # DI number

                    # index name

                    for i in range(
                        CURRENT_EXTIO_DI_NUM,
                        CURRENT_EXTIO_DI_NUM + extio_pin_setup[0][1],
                    ):
                        diInfo = extioModConfig.diIf.add()
                        diInfo.index = i
                        if str(i) in SET_NAME_EXTIO_DI:
                            diInfo.name = SET_NAME_EXTIO_DI[str(i)]
                        else:
                            diInfo.name = "DI_" + str(i + 1)
                        for idx in range(0, FUNCDI_NUM):
                            if (
                                currFuncDIMap[idx][0] == 2
                                and currFuncDIMap[idx][1] == i
                            ):
                                diInfo.diFuncIO = idx + 1
                                break

                    for i in range(
                        CURRENT_EXTIO_DO_NUM,
                        CURRENT_EXTIO_DO_NUM + extio_pin_setup[1][1],
                    ):
                        doInfo = extioModConfig.doIf.add()
                        doInfo.index = i
                        if str(i) in SET_NAME_EXTIO_DO:
                            doInfo.name = SET_NAME_EXTIO_DO[str(i)]
                        else:
                            doInfo.name = "DO_" + str(i + 1)
                        flag = 0
                        for idx in range(0, FUNCDO_NUM):
                            for idy in range(0, 4):
                                if (
                                    currFuncDOMap[idx][idy][0] == 2
                                    and currFuncDOMap[idx][idy][1] == i
                                ):
                                    doInfo.doFuncIO = idx + 1
                                    flag = 1
                                    break
                            if flag == 1:
                                flag = 0
                                break

                    for i in range(
                        CURRENT_EXTIO_AI_NUM,
                        CURRENT_EXTIO_AI_NUM + extio_pin_setup[2][1],
                    ):
                        aiInfo = extioModConfig.aiIf.add()
                        aiInfo.index = i
                        if str(i) in SET_NAME_EXTIO_AI:
                            aiInfo.name = SET_NAME_EXTIO_AI[str(i)]
                        else:
                            aiInfo.name = "AI_" + str(i + 1)

                    for i in range(
                        CURRENT_EXTIO_AO_NUM,
                        CURRENT_EXTIO_AO_NUM + extio_pin_setup[3][1],
                    ):
                        aoInfo = extioModConfig.aoIf.add()
                        aoInfo.index = i
                        if str(i) in SET_NAME_EXTIO_AO:
                            aoInfo.name = SET_NAME_EXTIO_AO[str(i)]
                        else:
                            aoInfo.name = "AO_" + str(i + 1)

                    CURRENT_EXTIO_DI_NUM += extio_pin_setup[0][1]
                    CURRENT_EXTIO_DO_NUM += extio_pin_setup[1][1]
                    CURRENT_EXTIO_AI_NUM += extio_pin_setup[2][1]
                    CURRENT_EXTIO_AO_NUM += extio_pin_setup[3][1]

            if ioType == 4 or ioType == -1:  # Modbus slave
                grpc_logger().debug("Modbus io")
                # c.set_io_type(4)
                ioNum = s.get_io_num(4)

                ioConfigInfo.mbSlaveIOConfigInfo.diNum = ioNum[0]
                modMaxDIO = ioNum[0]
                ioConfigInfo.mbSlaveIOConfigInfo.doNum = ioNum[1]
                ioConfigInfo.mbSlaveIOConfigInfo.aiNum.ioNum_uint = ioNum[2]
                ioConfigInfo.mbSlaveIOConfigInfo.aiNum.ioNum_int = ioNum[3]
                ioConfigInfo.mbSlaveIOConfigInfo.aiNum.ioNum_float = ioNum[4]
                modMaxAIO = ioNum[2] + ioNum[3] + ioNum[4]
                ioConfigInfo.mbSlaveIOConfigInfo.aoNum.ioNum_uint = ioNum[5]
                ioConfigInfo.mbSlaveIOConfigInfo.aoNum.ioNum_int = ioNum[6]
                ioConfigInfo.mbSlaveIOConfigInfo.aoNum.ioNum_float = ioNum[7]
                # index name
                for i in range(ioNum[0]):
                    diInfo = ioConfigInfo.mbSlaveIOConfigInfo.diIf.add()
                    diInfo.index = i
                    if str(i) in SET_NAME_MB_DI:
                        diInfo.name = SET_NAME_MB_DI[str(i)]
                    elif str(i + cabMaxDIO) in SET_NAME_STDIO_DI:
                        diInfo.name = SET_NAME_STDIO_DI[str(i + cabMaxDIO)]
                    else:
                        diInfo.name = "DI_" + str(i + 1)
                    for idx in range(0, FUNCDI_NUM):
                        if (
                            currFuncDIMap[idx][0] == 4 and currFuncDIMap[idx][1] == i
                        ) or (
                            currFuncDIMap[idx][0] == 0
                            and currFuncDIMap[idx][1] == i + cabMaxDIO
                        ):
                            diInfo.diFuncIO = idx + 1
                            break

                # index name
                for i in range(ioNum[1]):
                    doInfo = ioConfigInfo.mbSlaveIOConfigInfo.doIf.add()
                    doInfo.index = i
                    if str(i) in SET_NAME_MB_DO:
                        doInfo.name = SET_NAME_MB_DO[str(i)]
                    elif str(i + cabMaxDIO) in SET_NAME_STDIO_DO:
                        doInfo.name = SET_NAME_STDIO_DO[str(i + cabMaxDIO)]
                    else:
                        doInfo.name = "DO_" + str(i + 1)
                    flag = 0
                    for idx in range(0, FUNCDO_NUM):
                        for idy in range(0, 4):
                            if (
                                currFuncDOMap[idx][idy][0] == 4
                                and currFuncDOMap[idx][idy][1] == i
                            ) or (
                                currFuncDOMap[idx][idy][0] == 0
                                and currFuncDOMap[idx][idy][1] == i + cabMaxDIO
                            ):
                                doInfo.doFuncIO = idx + 1
                                flag = 1
                                break
                        if flag == 1:
                            flag = 0
                            break

                # index name
                for i in range(ioNum[2] + ioNum[3] + ioNum[4]):
                    aiInfo = ioConfigInfo.mbSlaveIOConfigInfo.aiIf.add()
                    aiInfo.index = i
                    if str(i) in SET_NAME_MB_AI:
                        aiInfo.name = SET_NAME_MB_AI[str(i)]
                    elif str(i + cabMaxAI) in SET_NAME_STDIO_AI:
                        aiInfo.name = SET_NAME_STDIO_AI[str(i + cabMaxAI)]
                    else:
                        aiInfo.name = "AI_" + str(i + 1)
                # index name
                for i in range(ioNum[5] + ioNum[6] + ioNum[7]):
                    aoInfo = ioConfigInfo.mbSlaveIOConfigInfo.aoIf.add()
                    aoInfo.index = i
                    if str(i) in SET_NAME_MB_AO:
                        aoInfo.name = SET_NAME_MB_AO[str(i)]
                    elif str(i + cabMaxAO) in SET_NAME_STDIO_AO:
                        aoInfo.name = SET_NAME_STDIO_AO[str(i + cabMaxAO)]
                    else:
                        aoInfo.name = "AO_" + str(i + 1)

            if ioType == 5 or ioType == -1:  # Profinet IO
                grpc_logger().debug("Profinet io")
                # c.set_io_type(5)
                ioNum = s.get_io_num(5)

                ioConfigInfo.pnDevIOConfigInfo.diNum = ioNum[0]
                ioConfigInfo.pnDevIOConfigInfo.doNum = ioNum[1]
                ioConfigInfo.pnDevIOConfigInfo.aiNum.ioNum_int = ioNum[2]
                ioConfigInfo.pnDevIOConfigInfo.aiNum.ioNum_float = ioNum[3]
                ioConfigInfo.pnDevIOConfigInfo.aoNum.ioNum_int = ioNum[4]
                ioConfigInfo.pnDevIOConfigInfo.aoNum.ioNum_float = ioNum[5]
                # index name
                for i in range(ioNum[0]):
                    diInfo = ioConfigInfo.pnDevIOConfigInfo.diIf.add()
                    diInfo.index = i
                    if str(i) in SET_NAME_PFN_DI:
                        diInfo.name = SET_NAME_PFN_DI[str(i)]
                    elif str(i + cabMaxDIO + modMaxDIO) in SET_NAME_STDIO_DI:
                        diInfo.name = SET_NAME_STDIO_DI[str(i + cabMaxDIO + modMaxDIO)]
                    else:
                        diInfo.name = "DI_" + str(i + 1)
                    for idx in range(0, FUNCDI_NUM):
                        if (
                            currFuncDIMap[idx][0] == 5 and currFuncDIMap[idx][1] == i
                        ) or (
                            currFuncDIMap[idx][0] == 0
                            and currFuncDIMap[idx][1] == i + cabMaxDIO + modMaxDIO
                        ):
                            diInfo.diFuncIO = idx + 1
                            break

                # index name
                for i in range(ioNum[1]):
                    doInfo = ioConfigInfo.pnDevIOConfigInfo.doIf.add()
                    doInfo.index = i
                    if str(i) in SET_NAME_PFN_DO:
                        doInfo.name = SET_NAME_PFN_DO[str(i)]
                    elif str(i + cabMaxDIO + modMaxDIO) in SET_NAME_STDIO_DO:
                        doInfo.name = SET_NAME_STDIO_DO[str(i + cabMaxDIO + modMaxDIO)]
                    else:
                        doInfo.name = "DO_" + str(i + 1)
                    flag = 0
                    for idx in range(0, FUNCDO_NUM):
                        for idy in range(0, 4):
                            if (
                                currFuncDOMap[idx][idy][0] == 5
                                and currFuncDOMap[idx][idy][1] == i
                            ) or (
                                currFuncDOMap[idx][idy][0] == 0
                                and currFuncDOMap[idx][idy][1]
                                == i + cabMaxDIO + modMaxDIO
                            ):
                                doInfo.doFuncIO = idx + 1
                                flag = 1
                                break
                        if flag == 1:
                            flag = 0
                            break

                # index name
                for i in range(ioNum[2] + ioNum[3]):
                    aiInfo = ioConfigInfo.pnDevIOConfigInfo.aiIf.add()
                    aiInfo.index = i
                    if str(i) in SET_NAME_PFN_AI:
                        aiInfo.name = SET_NAME_PFN_AI[str(i)]
                    elif str(i + cabMaxAI + modMaxAIO) in SET_NAME_STDIO_AI:
                        aiInfo.name = SET_NAME_STDIO_AI[str(i + cabMaxAI + modMaxAIO)]
                    else:
                        aiInfo.name = "AI_" + str(i + 1)
                # index name
                for i in range(ioNum[4] + ioNum[5]):
                    aoInfo = ioConfigInfo.pnDevIOConfigInfo.aoIf.add()
                    aoInfo.index = i
                    if str(i) in SET_NAME_PFN_AO:
                        aoInfo.name = SET_NAME_PFN_AO[str(i)]
                    elif str(i + cabMaxAO + modMaxAIO) in SET_NAME_STDIO_AO:
                        aoInfo.name = SET_NAME_STDIO_AO[str(i + cabMaxAO + modMaxAIO)]
                    else:
                        aoInfo.name = "AO_" + str(i + 1)

            if ioType == 6 or ioType == -1:  # Ethernet/IP IO
                grpc_logger().debug("Ethernet/IP io")
                # c.set_io_type(6)
                ioNum = s.get_io_num(6)
                global EIP_AI_INT_NUMBER
                global EIP_AO_INT_NUMBER
                ioConfigInfo.eipIOConfigInfo.diNum = ioNum[0]
                ioConfigInfo.eipIOConfigInfo.doNum = ioNum[1]
                ioConfigInfo.eipIOConfigInfo.aiNum.ioNum_int = ioNum[2]
                EIP_AI_INT_NUMBER = ioNum[2]
                ioConfigInfo.eipIOConfigInfo.aiNum.ioNum_float = ioNum[3]
                ioConfigInfo.eipIOConfigInfo.aoNum.ioNum_int = ioNum[4]
                EIP_AO_INT_NUMBER = ioNum[4]
                ioConfigInfo.eipIOConfigInfo.aoNum.ioNum_float = ioNum[5]

                # index name
                for i in range(ioNum[0]):
                    diInfo = ioConfigInfo.eipIOConfigInfo.diIf.add()
                    diInfo.index = i
                    if str(i) in SET_NAME_EIP_DI:
                        diInfo.name = SET_NAME_EIP_DI[str(i)]
                    else:
                        diInfo.name = "DI_" + str(i + 1)
                    for idx in range(0, FUNCDI_NUM):
                        if currFuncDIMap[idx][0] == 6 and currFuncDIMap[idx][1] == i:
                            diInfo.diFuncIO = idx + 1
                            break

                # index nameCabinet
                for i in range(ioNum[1]):
                    doInfo = ioConfigInfo.eipIOConfigInfo.doIf.add()
                    doInfo.index = i
                    if str(i) in SET_NAME_EIP_DO:
                        doInfo.name = SET_NAME_EIP_DO[str(i)]
                    else:
                        doInfo.name = "DO_" + str(i + 1)
                    flag = 0
                    for idx in range(0, FUNCDO_NUM):
                        for idy in range(0, 4):
                            if (
                                currFuncDOMap[idx][idy][0] == 6
                                and currFuncDOMap[idx][idy][1] == i
                            ):
                                doInfo.doFuncIO = idx + 1
                                flag = 1
                                break
                        if flag == 1:
                            flag = 0
                            break

                # index name
                for i in range(ioNum[2] + ioNum[3]):
                    aiInfo = ioConfigInfo.eipIOConfigInfo.aiIf.add()
                    aiInfo.index = i
                    if str(i) in SET_NAME_EIP_AI:
                        aiInfo.name = SET_NAME_EIP_AI[str(i)]
                    else:
                        aiInfo.name = "AI_" + str(i + 1)

                # index name
                for i in range(ioNum[4] + ioNum[5]):
                    aoInfo = ioConfigInfo.eipIOConfigInfo.aoIf.add()
                    aoInfo.index = i
                    if str(i) in SET_NAME_EIP_AO:
                        aoInfo.name = SET_NAME_EIP_AO[str(i)]
                    else:
                        aoInfo.name = "AO_" + str(i + 1)

            return jaka_pb2.Response(errcode=0, ioConfigInfo=ioConfigInfo)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetIOState(self, request, context):
        try:
            ioState = jaka_pb2.IOState()
            update_jkzuc_status()
            ioType = int(request.ioState.ioType)
            # ioType = 2
            cnt = 0

            if ioType == 0 or ioType == -1:  # Cabinet IO
                # grpc_logger().debug('Cabinet io')
                for i in s.cab_din:
                    ioState.cabIOState.di.append(i)
                for i in s.cab_dout:
                    ioState.cabIOState.do.append(i)
                for i in s.cab_ain:
                    ioState.cabIOState.ai.append(i)
                for i in s.cab_aout:
                    ioState.cabIOState.ao.append(i)

            if ioType == 1 or ioType == -1:  #Tool IO
                # grpc_logger().debug('Tool io')
                for i in s.tio_din:
                    ioState.tioState.tio_din.append(i)
                for i in s.tio_dout:
                    ioState.tioState.tio_dout.append(i)
                for i in s.tio_ain:
                    ioState.tioState.tio_ain.append(i)

            if ioType == 2 or ioType == -1:  # extio Modbus master
                # grpc_logger().debug('extio')
                for i in s.extio_stat[0]:
                    ioState.extIOState.extio_din.append(i)
                for i in s.extio_stat[1]:
                    ioState.extIOState.extio_dout.append(i)
                for i in s.extio_stat[2]:
                    ioState.extIOState.extio_ain.append(i)
                for i in s.extio_stat[3]:
                    ioState.extIOState.extio_aout.append(i)

            if ioType == 4 or ioType == -1:  #Modbus slave IO
                # grpc_logger().debug('Modbus io')
                for i in s.mb_slave_din:
                    ioState.mbSlaveIOState.di.append(i)
                for i in s.mb_slave_dout:
                    ioState.mbSlaveIOState.do.append(i)
                for i in s.mb_slave_ain:
                    if cnt < 16:
                        ioState.mbSlaveIOState.ai_uint.append(int(i))
                    elif cnt < 32:
                        ioState.mbSlaveIOState.ai_int.append(int(i))
                    else:
                        ioState.mbSlaveIOState.ai_float.append(i)
                    cnt = cnt + 1

                cnt = 0
                for i in s.mb_slave_aout:
                    if cnt < 16:
                        ioState.mbSlaveIOState.ao_uint.append(int(i))
                    elif cnt < 32:
                        ioState.mbSlaveIOState.ao_int.append(int(i))
                    else:
                        ioState.mbSlaveIOState.ao_float.append(i)
                    cnt = cnt + 1

            if ioType == 5 or ioType == -1:  # Profinet IO
                # grpc_logger().debug('Profinet io')
                for i in s.pn_dev_din:
                    ioState.pnDevIOState.di.append(i)
                for i in s.pn_dev_dout:
                    ioState.pnDevIOState.do.append(i)

                cnt = 0
                for i in s.pn_dev_ain:
                    if cnt < 32:
                        ioState.pnDevIOState.ai_int.append(int(i))
                    else:
                        ioState.pnDevIOState.ai_float.append(i)
                    cnt = cnt + 1

                cnt = 0
                for i in s.pn_dev_aout:
                    if cnt < 32:
                        ioState.pnDevIOState.ao_int.append(int(i))
                    else:
                        ioState.pnDevIOState.ao_float.append(i)
                    cnt = cnt + 1

            if ioType == 6 or ioType == -1:  # Ethernet/IP IO
                global EIP_AI_INT_NUMBER
                global EIP_AO_INT_NUMBER
                for i in s.eip_adpt_din:
                    ioState.eipIOState.di.append(i)
                for i in s.eip_adpt_dout:
                    ioState.eipIOState.do.append(i)
                cnt = 0
                for i in s.eip_adpt_ain:
                    if cnt < EIP_AI_INT_NUMBER:
                        ioState.eipIOState.ai_int.append(int(i))
                    else:
                        ioState.eipIOState.ai_float.append(i)
                    cnt = cnt + 1

                cnt = 0
                for i in s.eip_adpt_aout:
                    if cnt < EIP_AO_INT_NUMBER:
                        ioState.eipIOState.ao_int.append(int(i))
                    else:
                        ioState.eipIOState.ao_float.append(i)
                    cnt = cnt + 1

            return jaka_pb2.Response(errcode=0, ioState=ioState)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetHomeVelocity(self, request, context):
        """
        设置三种回预设姿态时使用的速度百分比。
        :param request: homeVel: 速度百分比
        """
        grpc_logger().debug(
            "SetHomeVelocity request.homeVel:{}".format(request.homeVel)
        )
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.homeVel > 0 and request.homeVel <= 100:
                c.set_home_vel(request.homeVel)
                return jaka_pb2.Response(errcode=0)
            return jaka_pb2.Response(
                errcode=jaka_pb2.RPC_INVALID_ARGS,
                errormsg="Exception:invalid parameters",
            )
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetHomeVelocity(self, request, context):
        """
        获取三种回预设姿态时使用的速度百分比。
        :return: homeVel: 速度百分比
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            update_jkzuc_status()
            return jaka_pb2.Response(errcode=0, homeVel=s.home_vel)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetRobotSimEnable(self, request, context):
        """
        设置是否开启仿真模式。
        :param request: intid: 0:关闭仿真模式，1:开启仿真模式
        """
        grpc_logger().debug("SetRobotSimEnable request.intid:{}".format(request.intid))
        try:
            c.set_sim_enable(request.intid)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetRobotSimEnable(self, request, context):
        """
        获取是否开启仿真模式。
        :return: intid: 0:关闭仿真模式，1:开启仿真模式
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            s.poll()
            return jaka_pb2.Response(errcode=0, intid=s.sim_mode)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def SetSimRobot(self, request, context):
        """
        设置仿真机器人模型
        :param request: intid: 仿真机器人模型索引
        """
        grpc_logger().debug("SetSimRobot request.intid:{}".format(request.intid))
        try:
            if s.sim_mode != 1:
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc call failed"
                )

            sim_model_list = SIMROBOTMODEL_MAP
            grpc_logger().info(request.intid)
            serial_num = sim_model_list[request.intid][0]
            c.set_sim_robot(request.robot_index, serial_num)
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetSimRobot(self, request, context):
        """
        获取当前仿真机器人模型
        :return: intid: 仿真机器人模型索引
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            sim_model_list = SIMROBOTMODEL_MAP
            index = -1
            s.poll()
            robotModel = s.current_robot_model
            for item in sim_model_list:
                index = index + 1
                if item[0] / 10000 == robotModel[0] / 10000:
                    break
            if index >= len(sim_model_list) or index < 0:
                grpc_logger().error("[grpc] err index:%d".format(index))
                return jaka_pb2.Response(
                    errcode=1, errormsg="Exception:grpc get current robot model failed."
                )
            grpc_logger().debug("[grpc] suceess index:%d".format(index))
            return jaka_pb2.Response(errcode=0, intid=index)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetRobotModelList(self, request, context):
        """
        获取仿真机器人模型列表
        :return: robotModelList: 仿真机器人模型列表
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            sim_model_list = SIMROBOTMODEL_MAP
            modelList = jaka_pb2.RobotModelList()
            for index in range(len(sim_model_list)):
                robotName = sim_model_list[index][1]
                modelList.robotNames.append(robotName)
                # robotName
            return jaka_pb2.Response(errcode=0, robotModelList=modelList)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    ################################ grpc server interfaces end ###################################
    @login_required
    def getRobotInfo(self, request, context):
        """
        用于让app获取序列号，而不是从udp接口中获取。仅借用了brd的信息打包接口，以保证成功
        """
        s.poll()
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        rob = {}
        rob["robId"] = brd.robotIdFormat(s.robot_serial_num)
        rob["interpStat"] = s.interp_state
        rob["name"] = s.robot_name
        rob["connectStat"] = True
        rob["ver"] = jkzuc.controller_version
        rob["errcode"] = 0
        info = brd.robotInfoFormat(rob, "00-00-00-00-00-00")
        return jaka_pb2.RobotInfoResp(infostr=info, errcode=0)

    def handleRequest(self, request, context):
        try:
            global HEARTBEAT
            global CURRENT_TIME
            if not HEARTBEAT:
                self.LOGSTATUS = False
                Robot.client_uq_IPA = ""
            req_type = request.reqtype
            # grpc_logger().debug("message id: {}".format(req_type))
            client_ip = context.peer().split(":")[1]
            if "127.0.0.1" != client_ip:  # if not loop client(addon)
                CURRENT_TIME = time.time()
                if (context.peer().split(":")[1] != Robot.client_uq_IPA) and (
                    True == self.LOGSTATUS
                ):  # check valid first
                    if req_type == jaka_pb2.MSGID_LOGIN:
                        if client_ip != self.clientIP:
                            return jaka_pb2.Response(
                                errcode=4, errormsg="Login failed:Repeat login", intid=0
                            )  # 被占用
                        else:
                            return jaka_pb2.Response(
                                errcode=4, errormsg="Login failed:Repeat login", intid=1
                            )  # 重复登录

                    context.set_code(grpc.StatusCode.CANCELLED)  ##only set context
                    return jaka_pb2.Response(errcode=65535, errormsg="Other error.")
                if (
                    req_type < jaka_pb2.MSGID_LOGIN
                    or req_type >= jaka_pb2.MSGID_MAXIMUM
                ):  # check MSGID valid
                    return jaka_pb2.Response(errcode=16, errormsg="Msgid out of range")
                if (
                    req_type != jaka_pb2.MSGID_LOGIN
                ):  # login status check TODO this flag was attributed to unsafe
                    if not self.LOGSTATUS:
                        return jaka_pb2.Response(
                            resptype=req_type, errcode=5, errormsg="Please login first."
                        )
                if (self.userlevel == "visitor") and (
                    req_type not in self.VISITOR_REQ_LIST
                ):  # user auth check
                    print(
                        "-----self.userlevel：{}  type  {}".format(
                            self.userlevel, req_type
                        )
                    )
                    return jaka_pb2.Response(
                        resptype=req_type, errcode=18, errormsg="Permission denied."
                    )
                HEARTBEAT = True
            response = self.func_name_dict[req_type](request, context)
            response.resptype = req_type
            return response
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=65535, errormsg="Other error.")

    def uploadFile(self, request_iterator, context):
        """
        上传文件
        :param request_iterator: 文件流
        :param context: 上下文
        :return: 文件大小
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        shared_resource_lock.acquire()
        try:
            file_name = ""
            grpc_logger().debug("[gRPC] uploadFile ...")
            with open(jkzuc.jkroot_dir + "/scripts/tmp_file", "w+") as fd:
                for line in iter(request_iterator):
                    fd.write(line.filetext)
                    file_name = line.filename
                # flush the buffer
                fd.flush()
                fd.close()
                self.sync_path(jkzuc.jkroot_dir + "/scripts/tmp_file")

                folder_list = file_name.split("/")
                del folder_list[-1]
                folder_path = ""
                for folder in folder_list:
                    folder_path += folder + "/"
                scripts_path = jkzuc.jkroot_dir + "/scripts/"
                addon_path = jkzuc.jkroot_dir + "/configs/JAKA/AddOns/"
                filePath = scripts_path + folder_path
                real_path = os.path.abspath(filePath)
                if not (scripts_path in real_path or addon_path in real_path):
                    context.set_code(grpc.StatusCode.CANCELLED)
                    return
                if not os.path.exists(jkzuc.jkroot_dir + "/scripts/" + folder_path):
                    os.makedirs(jkzuc.jkroot_dir + "/scripts/" + folder_path)
                file_path = jkzuc.jkroot_dir + "/scripts/" + file_name
                # os.rename(jkzuc.jkroot_dir + '/scripts/tmp_file',file_path)
                shutil.move(jkzuc.jkroot_dir + "/scripts/tmp_file", file_path)
                self.sync_path(file_path)
                count = os.path.getsize(file_path)
                grpc_logger().debug(
                    "[gRPC] uploadFile: complete uploading file {}, size {:d}".format(
                        file_path, count
                    )
                )

                global MTN_ABORT
                MTN_ABORT = False
            shared_resource_lock.release()
            c.reset_step_status()
            return jaka_pb2.FileSize(errcode=0, size=count)
        except:
            grpc_logger().error(traceback.format_exc())
            shared_resource_lock.release()
            return jaka_pb2.FileSize(errcode=1, errormsg="Exception:grpc call failed")

    def downloadFile(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            scripts_path = jkzuc.jkroot_dir + "/scripts/"
            valid_path = jkzuc.jkroot_dir + "/"
            filePath = scripts_path + request.filename
            real_path = os.path.abspath(filePath)
            if (valid_path in real_path) == False:
                context.set_code(grpc.StatusCode.CANCELLED)
                return
            with open(filePath, "r") as fd:
                file_data = ""
                for line in fd.readlines():
                    file_data += line
                yield jaka_pb2.FileText(errcode=0, filetext=file_data)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return

    def uploadFileInByte(self, request_iterator, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        shared_resource_lock.acquire()
        try:
            file_name = ""
            with open(jkzuc.jkroot_dir + "/scripts/tmp_file", "wb+") as fd:
                for chunck in request_iterator:
                    fd.write(chunck.filedata)
                    file_name = chunck.filename
                self.sync_path(jkzuc.jkroot_dir + "/scripts/tmp_file")

                folder_list = file_name.split("/")
                del folder_list[-1]
                folder_path = ""
                for folder in folder_list:
                    folder_path += folder + "/"

                if not os.path.exists(jkzuc.jkroot_dir + "/scripts/" + folder_path):
                    os.makedirs(jkzuc.jkroot_dir + "/scripts/" + folder_path)
                file_path = jkzuc.jkroot_dir + "/scripts/" + file_name
                grpc_logger().debug("file path: {}".format(file_path))
                os.rename(jkzuc.jkroot_dir + "/scripts/tmp_file", file_path)
                self.sync_path(file_path)
                count = os.path.getsize(file_path)
                grpc_logger().debug(
                    "[gRPC] uploadFileInByte: complete uploading file {}, size {:d}".format(
                        file_path, count
                    )
                )
            shared_resource_lock.release()
            return jaka_pb2.FileSize(errcode=0, size=count)
        except:
            grpc_logger().error(traceback.format_exc())
            shared_resource_lock.release()
            return jaka_pb2.FileSize(errcode=1, errormsg="Exception:grpc call failed")

    # TODO 1) useful?which client version willuse this API
    def downloadFileInByte(self, request, context):
        # grpc_logger().debug('GRPC::{}'.format(sys._getframe().f_code.co_name))
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            file_name = request.filename
            with open(jkzuc.jkroot_dir + "/scripts/" + file_name, "rb") as fd:
                while True:
                    piece = fd.read(1024)
                    if len(piece) == 0:
                        return
                    yield jaka_pb2.FileByteData(errcode=0, filedata=piece)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return

    @login_required
    def setAddonGlobalUrlName(self, request, context):
        """
        描述:设置addon全局参数
        参数:
            addoninfo.name:addon名称,空字符串表示无生效全局url
        返回值:
            errcode: 0成功 否则失败
            errmsg:非0时有具体描述, errcode 0时为空字符串
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        if self.userlevel == "visitor":
            return jaka_pb2.AddonGlobalRes(
                errcode=jaka_pb2.RPC_PERMISSION_DENY, errormsg="Permission denied."
            )

        try:
            kw = {"Addon": {"golbal_url": ""}}
            addon_g_inifile = jkzuc.jkroot_dir + "/configs/JAKA/AddOns/Addon_G.ini"
            # Todo 校验addon名称是否存在,这部分暂时依靠app
            filename = jkzuc.jkroot_dir + "/configs/JAKA/AddOns"
            if request.addoninfo.name == "":
                self.save_config(addon_g_inifile, kw)
            else:
                if request.addoninfo.name not in os.listdir(filename):
                    return jaka_pb2.AddonGlobalRes(
                        errcode=jaka_pb2.RPC_FILE_DIR_NOT_FOUND,
                        errormsg="RPC_FILE_DIR_NOT_FOUND",
                    )
                kw["Addon"]["golbal_url"] = request.addoninfo.name
                self.save_config(addon_g_inifile, kw)
            return jaka_pb2.AddonGlobalRes(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.AddonGlobalRes(errcode=1, errormsg="server inner error")

    @login_required
    def getAddonGlobalUrlName(self, request, context):
        """
        描述:查询addon全局参数
        参数:
            无
        返回值:
            addoninfo.name:addon名称,空字符串表示无生效全局url
            errcode: 0成功 否则失败
            errmsg:非0时有具体描述, errcode 0时为空字符串
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            addon_g_inifile = jkzuc.jkroot_dir + "/configs/JAKA/AddOns/Addon_G.ini"
            tmp_addoninfo = jaka_pb2.AddOnInfo()
            if not os.path.exists(addon_g_inifile):
                tmp_addoninfo.name = ""
                return jaka_pb2.AddonGlobalRes(errcode=0, addoninfo=tmp_addoninfo)
            tool_function_ctx = tool_function.ModuleCtx(grpc_logger)
            tmp_addoninfo.name = tool_function.read_ini_config(
                addon_g_inifile, "Addon", "golbal_url", module_ctx=tool_function_ctx
            )
            return jaka_pb2.AddonGlobalRes(errcode=0, addoninfo=tmp_addoninfo)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.AddonGlobalRes(errcode=1, errormsg="server inner error")

    def SetOperationMode(self, request, context):
        print("GRPC::", sys._getframe().f_code.co_name)
        try:
            s.poll()
            if (
                s.inpos != 1
                or s.motion_mode == jkzuc.TRAJ_MODE_DRAG
                or s.interp_state != jkzuc.INTERP_IDLE
            ):
                return jaka_pb2.Response(
                    errcode=jaka_pb2.RPC_ERR_SWITCH_OPERATION_MODE_IN_WRONG_STATE,
                    errormsg="switch operation mode in wrong state",
                )
            c.setOperationMode(request.intid)
            grpc_logger().debug(
                "[gRPC] setOperationMode: switch operation mode is {}".format(
                    request.intid
                )
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            print(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    # for now, for weichuangli only
    def GetOperationMode(self, request, context):
        print("GRPC::", sys._getframe().f_code.co_name)
        try:
            config = configparser.ConfigParser()
            setting = "/usr/etc/jkzuc/configs/JAKA/usersettings.ini"
            config.read(setting)
            temp = config.get("OPERATIONMODE", "MODE")
            opmode = int(temp)
            grpc_logger().debug(
                "[gRPC] getOperationMode: current operation mode is {}".format(opmode)
            )
            return jaka_pb2.Response(errcode=0, intid=opmode)
        except Exception:
            print(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    def GetOperationModeOption(self, request, context):
        print("GRPC::", sys._getframe().f_code.co_name)
        try:
            s.poll()
            opmodeenable = s.operation_mode_option
            grpc_logger().debug(
                "[gRPC] getOperationModeOption: current operation mode enable option is {}".format(
                    opmodeenable
                )
            )
            return jaka_pb2.Response(errcode=0, intid=opmodeenable)
        except Exception:
            print(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    @login_required
    def setRobotForceLimitValue(self, request, context):
        """
        描述:设置机器人碰撞灵敏度
        参数:
            force_limit, 0-100
        返回值:
            errcode: 0成功 否则失败
            errmsg:非0时有具体描述, errcode 0时为空字符串
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            if request.force_limit < 0 or request.force_limit > 100:
                return jaka_pb2.CollisionForceLimitRes(
                    errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
                )
            s.poll()
            if s.task_mode != TASK_MANUAL_MODE or s.inpos != 1:
                return jaka_pb2.CollisionForceLimitRes(
                    errcode=jaka_pb2.RPC_DISABLE_SETTING_SAFETY_PARAMETERS,
                    errormsg="Setting of safety parameters is disabled in non-manual mode",
                )

            c.set_force_limit_value(request.robot_index, request.force_limit)
            return jaka_pb2.CollisionForceLimitRes(errcode=jaka_pb2.RPC_OK)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.CollisionForceLimitRes(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="server inner error"
            )

    @login_required
    def getRobotForceLimitValue(self, request, context):
        """
        描述:查寻机器人碰撞灵敏度
        参数:
            null
        返回值:
            errcode: 0成功 否则失败
            errmsg:非0时有具体描述, errcode 0时为空字符串
            force_limit, 0-100
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            s.poll()
            value = s.force_limit_value
            return jaka_pb2.CollisionForceLimitRes(
                errcode=jaka_pb2.RPC_OK, force_limit=value
            )
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.CollisionForceLimitRes(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="server inner error"
            )

    @login_required
    def SetSafetyReduceParam(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        errinfo = jaka_pb2.ErrInfo()
        errinfo.errcode = 1
        errinfo.errormsg = "Exception:grpc call failed"
        try:
            reduce_setting = request.reduce_setting
            # todo
            c.set_reduce_safety_settings(
                reduce_setting.reduce_tcp_vel,
                reduce_setting.reduce_elbow_vel,
                reduce_setting.reduce_momentum,
                reduce_setting.reduce_power,
                reduce_setting.reduce_stopping_time,
                reduce_setting.reduce_stopping_dist,
            )
            errinfo.errcode = 0
            errinfo.errormsg = ""
            return jaka_pb2.CommonRes(errinfo=errinfo)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.CommonRes(errinfo=errinfo)

    @login_required
    def GetSafetyReduceParam(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        errinfo = jaka_pb2.ErrInfo()
        errinfo.errcode = 1
        errinfo.errormsg = "Exception:grpc call failed"
        try:
            # todo
            s.poll()
            reduce_setting = jaka_pb2.ReduceSetting()
            reduce_setting.reduce_tcp_vel = s.reduce_safety["tcp_speed"]
            reduce_setting.reduce_elbow_vel = s.reduce_safety["elbow_speed"]
            reduce_setting.reduce_momentum = s.reduce_safety["momentum_limit"]
            reduce_setting.reduce_power = s.reduce_safety["power_limit"]
            reduce_setting.reduce_stopping_time = s.reduce_safety["stopping_time"]
            reduce_setting.reduce_stopping_dist = s.reduce_safety["stopping_dist"]
            errinfo.errcode = 0
            errinfo.errormsg = ""
            return jaka_pb2.GetReduceRes(errinfo=errinfo, reduce_setting=reduce_setting)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.GetReduceRes(errinfo=errinfo)

    @login_required
    def SetDragSpeedLimitParam(self, request, context):
        print("GRPC:: {}".format(sys._getframe().f_code.co_name))
        errinfo = jaka_pb2.ErrInfo()
        errinfo.errcode = 1
        errinfo.errormsg = "Exception:grpc call failed"

        if self.userlevel == "visitor" or self.userlevel == "operator":
            errinfo.errcode = jaka_pb2.RPC_PERMISSION_DENY
            errinfo.errormsg = "Permission denied."
            return jaka_pb2.CommonRes(errinfo=errinfo)

        try:
            drag_tcp_speed_limit = request.drag_tcp_speed_limit
            # todo
            c.set_drag_speed_limit_settings(drag_tcp_speed_limit)
            errinfo.errcode = 0
            errinfo.errormsg = ""
            return jaka_pb2.CommonRes(errinfo=errinfo)
        except Exception:
            print(traceback.format_exc())
            return jaka_pb2.CommonRes(errinfo=errinfo)

    @login_required
    def GetDragSpeedLimitParam(self, request, context):
        print("GRPC:: {}".format(sys._getframe().f_code.co_name))
        # print(request)
        errinfo = jaka_pb2.ErrInfo()
        errinfo.errcode = 1
        errinfo.errormsg = "Exception:grpc call failed"
        try:
            # todo
            s.poll()
            drag_tcp_speed_limit = s.drag_speed_limit
            errinfo.errcode = 0
            errinfo.errormsg = ""
            return jaka_pb2.GetDragSpeedLimitRes(
                errinfo=errinfo, drag_tcp_speed_limit=drag_tcp_speed_limit
            )
        except Exception:
            print(traceback.format_exc())
            return jaka_pb2.GetDragSpeedLimitRes(errinfo=errinfo)

    @login_required
    def setJointCollisionSensitivity(self, request, context):
        """
        描述:设置机器人关节碰撞灵敏度
        参数:
            joint_sensitivity1-6, 0-100
        返回值:
            errcode: 0成功 否则失败
            errmsg:非0时有具体描述, errcode 0时为空字符串
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            s.poll()
            if s.sim_mode == 1:
                grpc_logger().debug(
                    "setCollisionDetectionEnable cannot be called in simmode"
                )
                return jaka_pb2.JointCollisionSensitivityRes(
                    errcode=jaka_pb2.RPC_UNSUPPORTED_COMMAND_IN_SIM,
                    errormsg="can't do that during sim mode",
                )
            if (
                request.joint_sensitivity1 < 0
                or request.joint_sensitivity1 > 100
                or request.joint_sensitivity2 < 0
                or request.joint_sensitivity2 > 100
                or request.joint_sensitivity3 < 0
                or request.joint_sensitivity3 > 100
                or request.joint_sensitivity4 < 0
                or request.joint_sensitivity4 > 100
                or request.joint_sensitivity5 < 0
                or request.joint_sensitivity5 > 100
                or request.joint_sensitivity6 < 0
                or request.joint_sensitivity6 > 100
                or request.joint_sensitivity7 < 0
                or request.joint_sensitivity7 > 100
            ):
                return jaka_pb2.JointCollisionSensitivityRes(
                    errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
                )

            c.set_collision_sensitivity(
                request.joint_sensitivity1,
                request.joint_sensitivity2,
                request.joint_sensitivity3,
                request.joint_sensitivity4,
                request.joint_sensitivity5,
                request.joint_sensitivity6,
                request.joint_sensitivity7,
            )
            return jaka_pb2.JointCollisionSensitivityRes(errcode=jaka_pb2.RPC_OK)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.JointCollisionSensitivityRes(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="server inner error"
            )

    @login_required
    def getJointCollisionSensitivity(self, request, context):
        """
        描述:查询机器人关节碰撞灵敏度
        参数:
            null
        返回值:
            errcode: 0成功 否则失败
            errmsg:非0时有具体描述, errcode 0时为空字符串
            joint_sensitivity1-6, 0-100
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            s.poll()
            value = s.joint_collision_sensitivity
            return jaka_pb2.JointCollisionSensitivityRes(
                errcode=jaka_pb2.RPC_OK,
                joint_sensitivity1=value[0],
                joint_sensitivity2=value[1],
                joint_sensitivity3=value[2],
                joint_sensitivity4=value[3],
                joint_sensitivity5=value[4],
                joint_sensitivity6=value[5],
                joint_sensitivity7=value[6],
            )
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.JointCollisionSensitivityRes(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="server inner error"
            )

    @login_required
    def setCollisionDetectionEnable(self, request, context):
        """
        描述:设置机器人碰撞检测开启或关闭
        参数:
            enbale, 0-1
        返回值:
            errcode: 0成功 否则失败
            errmsg:非0时有具体描述, errcode 0时为空字符串
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            s.poll()
            if s.sim_mode == 1:
                grpc_logger().debug(
                    "setCollisionDetectionEnable cannot be called in simmode"
                )
                return jaka_pb2.CollisionDectionEnableRes(
                    errcode=jaka_pb2.RPC_UNSUPPORTED_COMMAND_IN_SIM,
                    errormsg="can't do that during sim mode",
                )
            if request.enable != 0 and request.enable != 1:
                return jaka_pb2.CollisionDectionEnableRes(
                    errcode=jaka_pb2.RPC_EXCEPT, errormsg="Exception:grpc call failed"
                )

            c.set_collision_detection_enable(request.enable)
            return jaka_pb2.CollisionDectionEnableRes(errcode=jaka_pb2.RPC_OK)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.CollisionDectionEnableRes(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="server inner error"
            )

    @login_required
    def getCollisionDetectionEnable(self, request, context):
        """
        描述:查询机器人碰撞检测开启或关闭
        参数:

        返回值:
            enbale, 0-1
            errcode: 0成功 否则失败
            errmsg:非0时有具体描述, errcode 0时为空字符串
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            s.poll()
            value = s.collision_detection_enable
            return jaka_pb2.CollisionDectionEnableRes(
                errcode=jaka_pb2.RPC_OK, enable=value
            )
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.CollisionDectionEnableRes(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="server inner error"
            )

    def setTIOSigErrProgAct(self, request, context):
        """
        描述:设置TIO信号量查询错误时的程序状态
        参数:
            sig_name: 信号量的名称
            prog_stat: 信号量错误时程序的状态
        返回值:
            errcode: 0成功 否则失败
            errmsg:非0时有具体描述, errcode 0时为空字符串
            prog_act: 设置值
        """
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            c.set_TIO_RS485_sig_prog_stat(request.sig_name, request.prog_act)
            return jaka_pb2.ProgActRes(
                errcode=jaka_pb2.RPC_OK, prog_act=request.prog_act
            )
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.ProgActRes(
                errcode=jaka_pb2.RPC_EXCEPT, errormsg="server inner error"
            )

    def SetOperationMode(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            s.poll()
            c.setOperationMode(request.intid)
            grpc_logger().debug(
                "[gRPC] setOperationMode: switch operation mode is {}".format(
                    request.intid
                )
            )
            return jaka_pb2.Response(errcode=0)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")

    # for now, for weichuangli only
    def GetOperationMode(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        try:
            config = configparser.ConfigParser()
            setting = jkzuc.jkroot_dir + "/configs/JAKA/usersettings.ini"
            config.read(setting)
            temp = config.get("OPERATIONMODE", "MODE")
            opmode = int(temp)
            grpc_logger().debug(
                "[gRPC] getOperationMode: current operation mode is {}".format(opmode)
            )
            return jaka_pb2.Response(errcode=0, intid=opmode)
        except Exception:
            grpc_logger().error(traceback.format_exc())
            return jaka_pb2.Response(errcode=1, errormsg="Exception:grpc call failed")
    
    @login_required
    def GetRobTypeInfo(self, request, context):
        grpc_logger().debug("GRPC::{}".format(sys._getframe().f_code.co_name))
        ret = jaka_pb2.RobotTypeInfo()
        try:
            ret.err.errcode = jaka_pb2.RPC_OK
            ret.err.errormsg = ""
            ret.serial = jaka_pb2.RobotTypeInfo.RobotSerial.ZU_SERIAL
            ret.type = 7
            ret.version = 10
        except Exception:
            grpc_logger().error(traceback.format_exc())
            ret.err.errcode = jaka_pb2.RPC_EXCEPT
            ret.err.errormsg = "server inner error"
        return ret

def signal_handler(sig, frame):
    grpc_logger().debug("[AddOn] Keyboard interrupt received. Stop all Addon service.")
    addon_controller_inst.stop_all()
    exit(0)


def serve(serveripaddr, grpcport, inifile, udpport):
    signal.signal(signal.SIGINT, signal_handler)
    signal.signal(signal.SIGTERM, signal_handler)
    INI_FILE = inifile
    grpc_logger().info("server process id: {}".format(os.getpid()))
    # log the version info
    grpc_logger().debug(
        "[gRPC] controller version: {}".format(jkzuc.controller_version)
    )
    grpc_logger().debug("[gRPC] inifile in grpc_server: {}".format(INI_FILE))

    # 检测当前机器人的作业模式
    tool_function_ctx = tool_function.ModuleCtx(grpc_logger)
    tool_function.check_robot_work_mode(INI_FILE, tool_function_ctx)

    # 启动 grpc server
    server = grpc.server(futures.ThreadPoolExecutor(max_workers=10))
    jaka_pb2_grpc.add_RobotServicer_to_server(Robot(), server)
    server.add_insecure_port(serveripaddr + ":" + str(grpcport))
    server.start()
    grpc_logger().info("[GRPC] SERVER START {}".format(grpcport))

    global CONNECT_FLAG, HEARTBEAT, CURRENT_TIME
    global addon_controller_inst
    addon_controller_inst = addon.addon_controller(c, jkzuc.OPERATOR_ERROR)
    c.set_app_connection_status(0)

    try:
        while True:
            time.sleep(0.1)
            if HEARTBEAT:
                global IS_SETTING_CONTROLLER_BOX_TIME
                if IS_SETTING_CONTROLLER_BOX_TIME > 0:
                    time.sleep(SLEEP_INTERVAL / 100.0)
                    IS_SETTING_CONTROLLER_BOX_TIME = (
                        IS_SETTING_CONTROLLER_BOX_TIME + 1
                    )
                    if IS_SETTING_CONTROLLER_BOX_TIME > 100:
                        IS_SETTING_CONTROLLER_BOX_TIME = 0
                        grpc_logger().warn(
                            "heartbeat timeout {}s".format(
                                time.time() - CURRENT_TIME
                            )
                        )
                        CONNECT_FLAG = False
                        Robot.client_uq_IPA = ""
                        c.set_app_connection_status(0)
                        HEARTBEAT = False
                elif (CURRENT_TIME != 0) and (
                    time.time() - CURRENT_TIME > SLEEP_INTERVAL
                ):
                    grpc_logger().warn(
                        "heartbeat timeout {}s".format(
                            time.time() - CURRENT_TIME
                        )
                    )
                    CONNECT_FLAG = False
                    Robot.client_uq_IPA = ""
                    # grpc_logger().debug("[GRPC] connect_flag FALSE")
                    c.set_app_connection_status(0)
                    HEARTBEAT = False

    except KeyboardInterrupt:
        server.stop(0)


def mtnAbortProcess():
    try:
        update_jkzuc_status()

        # if s.enabled:
        if s.enabled or s.cat1_stop_program_paused:
            c.abort()
            c.teleop_enable(
                0
            )  # switch into free mode, to avoid that movj will take no effect
            time.sleep(0.05)
        c.reset_step_status()
        global MTN_ABORT
        MTN_ABORT = True
        return 0
    except Exception:
        grpc_logger().error(traceback.format_exc())
        return -1


def remoteCopyProcess(server_rootPath, src_path, dst_path, flag):
    grpc_logger().debug("copy {} to {}".format(src_path, dst_path))
    global remoteCopyState
    try:
        xfer = ftptransfer.Xfer()
        xfer.setFtpParams(server_rootPath, "jaka", "jaka12345")
        scripts_path = jkzuc.jkroot_dir + "/scripts/"
        if xfer.upload(scripts_path + src_path, "./program/" + dst_path, flag) == False:
            grpc_logger().debug("[gRPC] RemoteCopy: the directory is already existed!")
            # "{\"fixed_key\":[\"%s\",\"%d\",\"%s\",\"%d\",\"%s\",\"%d\"]}"
            c.set_grpc_errcode(
                jkzuc.GRPC_RESPONSE_ERROR,
                0x0F0135,
                'remoteCopy failed. # {"fixed_key":["'
                + "Localhost:/"
                + src_path
                + '","'
                + server_rootPath
                + ":/program/"
                + dst_path
                + '"]}',
                0xFF,
                1,
            )
            remoteCopyState = -1
        else:
            grpc_logger().debug(
                "[gRPC] RemoteCopy: complete from {} to {}:program/{}".format(
                    src_path, server_rootPath, dst_path
                )
            )
            c.set_grpc_errcode(
                jkzuc.GRPC_RESPONSE_INFO,
                0x0F0134,
                'remoteCopy successed. # {"fixed_key":["'
                + "Localhost:/"
                + src_path
                + '","'
                + server_rootPath
                + ":/program/"
                + dst_path
                + '"]}',
                0xFF,
                1,
            )
            time.sleep(0.2)
            remoteCopyState = 0
    except Exception:
        grpc_logger().error(traceback.format_exc())
        c.set_grpc_errcode(
            jkzuc.GRPC_RESPONSE_ERROR,
            0x0F0135,
            'remoteCopy failed. # {"fixed_key":["'
            + "Localhost:/"
            + src_path
            + '","'
            + server_rootPath
            + ":/program/"
            + dst_path
            + '"]}',
            0xFF,
            1,
        )
        remoteCopyState = -1
    return remoteCopyState


def GrpcServerStart():
    serveripaddr = "0.0.0.0"
    grpc_server_port = 50051
    udp_port = 54545
    inifile = jkzuc.jkroot_dir + "/configs/JAKA/usersettings.ini"
    serve(serveripaddr, grpc_server_port, inifile, udp_port)


# # for testing
if __name__ == "__main__":
    # start grpc server
    serveripaddr = "0.0.0.0"
    grpc_server_port = 50051
    inifile = jkzuc.jkroot_dir + "/configs/JAKA/usersettings.ini"
    c.set_app_connection_status(0)
    serve(serveripaddr, grpc_server_port, inifile, serveripaddr)
